From 4836935c07a59949ca63e734afa2d6e8bd5cbccd Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 30 Dec 2015 16:24:24 -0800 Subject: [PATCH] More cleanup --- Makefile | 2 +- src/canonical_machine.c | 2340 +++++++++++------------ src/canonical_machine.h | 1106 +++++------ src/config.c | 806 ++++---- src/config.h | 412 ++-- src/config_app.c | 1388 +++++++------- src/config_app.h | 72 +- src/controller.c | 420 ++-- src/controller.h | 96 +- src/cycle_homing.c | 653 +++---- src/cycle_jogging.c | 222 +-- src/cycle_probing.c | 359 ++-- src/encoder.c | 34 +- src/encoder.h | 114 +- src/gcode/gcode_circles2.h | 2 +- src/gcode/gcode_debug_tests.h | 12 +- src/gcode/gcode_tests.h | 2 +- src/gcode_parser.c | 821 ++++---- src/gpio.c | 146 +- src/gpio.h | 6 +- src/hardware.c | 170 +- src/hardware.h | 338 ++-- src/help.c | 6 +- src/help.h | 22 +- src/json_parser.c | 816 ++++---- src/json_parser.h | 62 +- src/kinematics.c | 42 +- src/main.c | 408 +--- src/network.c | 96 +- src/network.h | 8 +- src/persistence.c | 54 +- src/persistence.h | 18 +- src/plan_arc.c | 440 ++--- src/plan_arc.h | 68 +- src/plan_exec.c | 1070 +++++------ src/plan_line.c | 1156 +++++------ src/plan_zoid.c | 506 ++--- src/planner.c | 472 ++--- src/planner.h | 354 ++-- src/pwm.c | 152 +- src/pwm.h | 72 +- src/report.c | 728 +++---- src/report.h | 114 +- src/settings.h | 139 +- src/settings/settings_Ultimaker.h | 298 +-- src/settings/settings_cnc3040.h | 296 +-- src/settings/settings_default.h | 296 +-- src/settings/settings_openpnp.h | 256 +-- src/settings/settings_othermill.h | 334 ++-- src/settings/settings_probotixV90.h | 294 +-- src/settings/settings_shapeoko2.h | 314 +-- src/settings/settings_test.h | 336 ++-- src/settings/settings_zen7x12.h | 302 +-- src/spindle.c | 110 +- src/spindle.h | 10 +- src/status.c | 346 ++++ src/stepper.c | 1192 ++++++------ src/stepper.h | 616 +++--- src/switch.c | 304 ++- src/switch.h | 171 +- src/system.c | 114 +- src/system.h | 184 +- src/test.c | 117 +- src/test.h | 113 +- src/tests/test_001_smoke.h | 36 +- src/tests/test_002_homing.h | 6 +- src/tests/test_003_squares.h | 4 +- src/tests/test_004_arcs.h | 4 +- src/tests/test_005_dwell.h | 6 +- src/tests/test_006_feedhold.h | 4 +- src/tests/test_007_Mcodes.h | 4 +- src/tests/test_008_json.h | 4 +- src/tests/test_009_inverse_time.h | 4 +- src/tests/test_010_rotary.h | 4 +- src/tests/test_011_small_moves.h | 4 +- src/tests/test_012_slow_moves.h | 4 +- src/tests/test_013_coordinate_offsets.h | 4 +- src/tests/test_014_microsteps.h | 4 +- src/tests/test_050_mudflap.h | 4 +- src/tests/test_051_braid.h | 6 +- src/text_parser.c | 308 +-- src/text_parser.h | 82 +- src/tinyg.h | 609 +++--- src/util.c | 218 +-- src/util.h | 38 +- src/xio.c | 304 --- src/xio.h | 398 ---- src/xio/xio.c | 304 +++ src/xio/xio.h | 389 ++-- src/xio/xio_file.c | 84 +- src/xio/xio_file.h | 62 +- src/xio/xio_pgm.c | 116 +- src/xio/xio_rs485.c | 224 +-- src/xio/xio_spi.c | 552 +++--- src/xio/xio_spi.h | 105 +- src/xio/xio_usart.c | 554 +++--- src/xio/xio_usart.h | 25 - src/xio/xio_usb.c | 272 +-- src/xmega/xmega_eeprom.c | 648 +++---- src/xmega/xmega_eeprom.h | 120 +- src/xmega/xmega_init.c | 130 +- src/xmega/xmega_init.h | 2 +- src/xmega/xmega_interrupts.c | 12 +- src/xmega/xmega_interrupts.h | 4 +- src/xmega/xmega_rtc.c | 46 +- src/xmega/xmega_rtc.h | 16 +- 106 files changed, 13589 insertions(+), 14462 deletions(-) create mode 100644 src/status.c delete mode 100755 src/xio.c delete mode 100755 src/xio.h create mode 100755 src/xio/xio.c diff --git a/Makefile b/Makefile index d986f95..79bbcdf 100755 --- a/Makefile +++ b/Makefile @@ -12,7 +12,7 @@ CPP = avr-g++ COMMON = -mmcu=$(MCU) CFLAGS += $(COMMON) -CFLAGS += -gdwarf-2 -std=gnu99 -Wall -DF_CPU=$(CLOCK)UL -Os -funsigned-char +CFLAGS += -gdwarf-2 -std=gnu99 -Wall -Werror -DF_CPU=$(CLOCK)UL -Os -funsigned-char CFLAGS += -funsigned-bitfields -fpack-struct -fshort-enums CFLAGS += -MD -MP -MT $@ -MF build/dep/$(@F).d diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 986c57e..53f5d3b 100755 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -25,69 +25,72 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ + /* - * This 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 + * 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) + * 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) + * 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. + * 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 planner buffer. + * - 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 "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "text_parser.h" #include "canonical_machine.h" #include "plan_arc.h" @@ -100,17 +103,9 @@ #include "switch.h" #include "hardware.h" #include "util.h" -#include "xio.h" // for serial queue flush - -/*********************************************************************************** - **** STRUCTURE ALLOCATIONS ******************************************************** - ***********************************************************************************/ +#include "xio/xio.h" // for serial queue flush -cmSingleton_t cm; // canonical machine controller singleton - -/*********************************************************************************** - **** GENERIC STATIC FUNCTIONS AND VARIABLES *************************************** - ***********************************************************************************/ +cmSingleton_t cm; // canonical machine controller singleton // command execution callbacks from planner queue static void _exec_offset(float *value, float *flag); @@ -124,13 +119,6 @@ static void _exec_program_finalize(float *value, float *flag); static int8_t _get_axis(const index_t index); static int8_t _get_axis_type(const index_t index); -/*********************************************************************************** - **** CODE ************************************************************************* - ***********************************************************************************/ - -/******************************** - * Internal getters and setters * - ********************************/ /* * Canonical Machine State functions * @@ -142,47 +130,41 @@ static int8_t _get_axis_type(const index_t index); * cm_get_homing_state() * cm_set_motion_state() - adjusts active model pointer as well */ -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;} - else if (cm.cycle_state == CYCLE_HOMING) { cm.combined_state = COMBINED_HOMING;} - else if (cm.cycle_state == CYCLE_JOG) { cm.combined_state = COMBINED_JOG;} - else { - if (cm.motion_state == MOTION_RUN) cm.combined_state = COMBINED_RUN; - if (cm.motion_state == MOTION_HOLD) cm.combined_state = COMBINED_HOLD; - } - if (cm.machine_state == MACHINE_SHUTDOWN) { cm.combined_state = COMBINED_SHUTDOWN;} - - return cm.combined_state; -} - -uint8_t cm_get_machine_state() { return cm.machine_state;} -uint8_t cm_get_cycle_state() { return cm.cycle_state;} -uint8_t cm_get_motion_state() { return cm.motion_state;} -uint8_t cm_get_hold_state() { return cm.hold_state;} -uint8_t cm_get_homing_state() { return cm.homing_state;} - -void cm_set_motion_state(uint8_t motion_state) -{ - cm.motion_state = motion_state; - - switch (motion_state) { - case (MOTION_STOP): { ACTIVE_MODEL = MODEL; break; } - case (MOTION_RUN): { ACTIVE_MODEL = RUNTIME; break; } - case (MOTION_HOLD): { ACTIVE_MODEL = RUNTIME; break; } - } -} - -/*********************************** - * Model State Getters and Setters * - ***********************************/ - -/* 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 +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;} + else if (cm.cycle_state == CYCLE_HOMING) { cm.combined_state = COMBINED_HOMING;} + else if (cm.cycle_state == CYCLE_JOG) { cm.combined_state = COMBINED_JOG;} + else { + if (cm.motion_state == MOTION_RUN) cm.combined_state = COMBINED_RUN; + if (cm.motion_state == MOTION_HOLD) cm.combined_state = COMBINED_HOLD; + } + if (cm.machine_state == MACHINE_SHUTDOWN) { cm.combined_state = COMBINED_SHUTDOWN;} + + return cm.combined_state; +} + +uint8_t cm_get_machine_state() {return cm.machine_state;} +uint8_t cm_get_cycle_state() {return cm.cycle_state;} +uint8_t cm_get_motion_state() {return cm.motion_state;} +uint8_t cm_get_hold_state() {return cm.hold_state;} +uint8_t cm_get_homing_state() {return cm.homing_state;} + +void cm_set_motion_state(uint8_t motion_state) { + cm.motion_state = motion_state; + + switch (motion_state) { + case (MOTION_STOP): { ACTIVE_MODEL = MODEL; break; } + case (MOTION_RUN): { ACTIVE_MODEL = RUNTIME; break; } + case (MOTION_HOLD): { ACTIVE_MODEL = RUNTIME; break; } + } +} + +/* 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;} @@ -194,8 +176,8 @@ uint8_t cm_get_distance_mode(GCodeState_t *gcode_state) { return gcode_state->di 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_block_delete_switch() { return cm.gmx.block_delete_switch;} -uint8_t cm_get_runtime_busy() { return (mp_get_runtime_busy());} +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;} @@ -204,16 +186,14 @@ void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode) { gcod void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed) { gcode_state->spindle_speed = speed;} void cm_set_tool_number(GCodeState_t *gcode_state, uint8_t tool) { gcode_state->tool = tool;} -void cm_set_absolute_override(GCodeState_t *gcode_state, uint8_t absolute_override) -{ - gcode_state->absolute_override = absolute_override; - cm_set_work_offsets(MODEL); // must reset offsets if you change absolute override +void cm_set_absolute_override(GCodeState_t *gcode_state, uint8_t absolute_override) { + gcode_state->absolute_override = absolute_override; + cm_set_work_offsets(MODEL); // must reset offsets if you change absolute override } -void cm_set_model_linenum(uint32_t linenum) -{ - cm.gm.linenum = linenum; // you must first set the model line number, - nv_add_object((const char_t *)"n"); // then add the line number to the nv list +void cm_set_model_linenum(uint32_t linenum) { + cm.gm.linenum = linenum; // you must first set the model line number, + nv_add_object((const char_t *)"n"); // then add the line number to the nv list } /*********************************************************************************** @@ -225,269 +205,254 @@ void cm_set_model_linenum(uint32_t linenum) * 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. + * 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: - * - 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 - * - G28 and G30 moves; these are run in absolute coordinates + * - 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 + * - 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. * * 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 - * - A fully 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 + * - A fully 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 * - * 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 - 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 - return (offset); +float cm_get_active_coord_offset(uint8_t axis) { + if (cm.gm.absolute_override == true) return 0; // no offset if 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 + return offset; } + /* * cm_get_work_offset() - 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 (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 */ - -float cm_get_work_offset(GCodeState_t *gcode_state, uint8_t axis) -{ - return (gcode_state->work_offset[axis]); +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 * - * 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 (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 */ - -void cm_set_work_offsets(GCodeState_t *gcode_state) -{ - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - gcode_state->work_offset[axis] = cm_get_active_coord_offset(axis); - } +void cm_set_work_offsets(GCodeState_t *gcode_state) { + for (uint8_t axis = AXIS_X; axis < AXES; axis++) + gcode_state->work_offset[axis] = cm_get_active_coord_offset(axis); } + /* * cm_get_absolute_position() - 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 (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model + * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct * - * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's) - * NOTE: Machine position is always returned in mm mode. No units conversion is performed + * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's) + * NOTE: Machine position is always returned in mm mode. No units conversion is performed */ - -float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t axis) -{ - if (gcode_state == MODEL) return (cm.gmx.position[axis]); - return (mp_get_runtime_absolute_position(axis)); +float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t axis) { + if (gcode_state == MODEL) return cm.gmx.position[axis]; + return mp_get_runtime_absolute_position(axis); } /* * cm_get_work_position() - return work position in external form * - * ... that means in prevailing units (mm/inch) and with all offsets applied + * ... 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. + * 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 + * 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 * * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's) */ +float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis) { + float position; -float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis) -{ - float position; + if (gcode_state == MODEL) + position = cm.gmx.position[axis] - cm_get_active_coord_offset(axis); + else position = mp_get_runtime_work_position(axis); - if (gcode_state == MODEL) { - position = cm.gmx.position[axis] - cm_get_active_coord_offset(axis); - } else { - position = mp_get_runtime_work_position(axis); - } - if (gcode_state->units_mode == INCHES) { position /= MM_PER_INCH; } - return (position); + if (gcode_state->units_mode == INCHES) position /= MM_PER_INCH; + + return position; } + /*********************************************************************************** * CRITICAL HELPERS * Core functions supporting the canonical machining functions * These functions are not part of the NIST defined functions ***********************************************************************************/ + /* * cm_finalize_move() - perform final operations for a traverse or feed * cm_update_model_position_from_runtime() - set endpoint position from final runtime position * - * 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 + copy_vector(cm.gmx.position, cm.gm.target); // update model position - // if in ivnerse time mode reset feed rate so next block requires an explicit feed rate setting - if ((cm.gm.feed_rate_mode == INVERSE_TIME_MODE) && (cm.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED)) { - cm.gm.feed_rate = 0; - } + // if in ivnerse time mode reset feed rate so next block requires an explicit feed rate setting + if ((cm.gm.feed_rate_mode == INVERSE_TIME_MODE) && (cm.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED)) + cm.gm.feed_rate = 0; } -void cm_update_model_position_from_runtime() { copy_vector(cm.gmx.position, mr.gm.target); } + +void cm_update_model_position_from_runtime() {copy_vector(cm.gmx.position, mr.gm.target);} + /* * cm_deferred_write_callback() - write any changed G10 values back to persistence * - * Only runs if there is G10 data to write, there is no movement, and the serial queues are quiescent - * This could be made tighter by issuing an XOFF or ~CTS beforehand and releasing it afterwards. + * Only runs if there is G10 data to write, there is no movement, and the serial queues are quiescent + * This could be made tighter by issuing an XOFF or ~CTS beforehand and releasing it afterwards. */ +stat_t cm_deferred_write_callback() { + if ((cm.cycle_state == CYCLE_OFF) && (cm.deferred_write_flag == true)) { + if (xio_isbusy()) return STAT_OK; // don't write back if serial RX is not empty + cm.deferred_write_flag = false; + nvObj_t nv; + for (uint8_t i=1; i<=COORDS; i++) + for (uint8_t j=0; j DISABLE_SOFT_LIMIT) && (target[axis] < cm.a[axis].travel_min)) + return STAT_SOFT_LIMIT_EXCEEDED; - 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)) + return STAT_SOFT_LIMIT_EXCEEDED; + } + } - if ((cm.a[axis].travel_max > DISABLE_SOFT_LIMIT) && (target[axis] > cm.a[axis].travel_max)) { - return (STAT_SOFT_LIMIT_EXCEEDED); - } - } - } - return (STAT_OK); + return STAT_OK; } + /************************************************************************* * CANONICAL MACHINING FUNCTIONS - * Values are passed in pre-unit_converted state (from gn structure) - * All operations occur on gm (current model state) + * 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 @@ -496,110 +461,98 @@ stat_t cm_test_soft_limits(float target[]) /****************************************** * Initialization and Termination (4.3.2) * ******************************************/ -/* - * canonical_machine_init() - 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, 0, sizeof(cm)); // do not reset canonicalMachineSingleton once it's been initialized - memset(&cm.gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status - memset(&cm.gn, 0, sizeof(GCodeInput_t)); - memset(&cm.gf, 0, sizeof(GCodeInput_t)); +/// canonical_machine_init() - 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, 0, sizeof(cm)); // do not reset canonicalMachineSingleton once it's been initialized + memset(&cm.gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status + memset(&cm.gn, 0, sizeof(GCodeInput_t)); + memset(&cm.gf, 0, sizeof(GCodeInput_t)); - canonical_machine_init_assertions(); // establish assertions - ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer + canonical_machine_init_assertions(); // establish assertions + ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer - // set gcode defaults - cm_set_units_mode(cm.units_mode); - cm_set_coord_system(cm.coord_system); - cm_select_plane(cm.select_plane); - cm_set_path_control(cm.path_control); - cm_set_distance_mode(cm.distance_mode); - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);// always the default + // set gcode defaults + cm_set_units_mode(cm.units_mode); + cm_set_coord_system(cm.coord_system); + cm_select_plane(cm.select_plane); + cm_set_path_control(cm.path_control); + cm_set_distance_mode(cm.distance_mode); + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);// always the default - cm.gmx.block_delete_switch = true; + cm.gmx.block_delete_switch = true; - // never start a machine in a motion mode - cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; + // never start a machine in a motion mode + cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE; - // reset request flags - cm.feedhold_requested = false; - cm.queue_flush_requested = false; - cm.cycle_start_requested = false; + // reset request flags + cm.feedhold_requested = false; + cm.queue_flush_requested = false; + cm.cycle_start_requested = false; - // signal that the machine is ready for action - cm.machine_state = MACHINE_READY; - cm.combined_state = COMBINED_READY; + // signal that the machine is ready for action + cm.machine_state = MACHINE_READY; + cm.combined_state = COMBINED_READY; - // sub-system inits - cm_spindle_init(); - cm_arc_init(); + // sub-system inits + cm_spindle_init(); + cm_arc_init(); } + /* * canonical_machine_init_assertions() * canonical_machine_test_assertions() - test assertions, return error code if violation exists */ -void canonical_machine_init_assertions(void) -{ - cm.magic_start = MAGICNUM; - cm.magic_end = MAGICNUM; - cm.gmx.magic_start = MAGICNUM; - cm.gmx.magic_end = MAGICNUM; - arc.magic_start = MAGICNUM; - arc.magic_end = MAGICNUM; +void canonical_machine_init_assertions() { + cm.magic_start = MAGICNUM; + cm.magic_end = MAGICNUM; + cm.gmx.magic_start = MAGICNUM; + cm.gmx.magic_end = MAGICNUM; + arc.magic_start = MAGICNUM; + arc.magic_end = MAGICNUM; } -stat_t canonical_machine_test_assertions(void) -{ - if ((cm.magic_start != MAGICNUM) || (cm.magic_end != MAGICNUM)) return (STAT_CANONICAL_MACHINE_ASSERTION_FAILURE); - if ((cm.gmx.magic_start != MAGICNUM) || (cm.gmx.magic_end != MAGICNUM)) return (STAT_CANONICAL_MACHINE_ASSERTION_FAILURE); - if ((arc.magic_start != MAGICNUM) || (arc.magic_end != MAGICNUM)) return (STAT_CANONICAL_MACHINE_ASSERTION_FAILURE); - return (STAT_OK); + +stat_t canonical_machine_test_assertions() { + if ((cm.magic_start != MAGICNUM) || (cm.magic_end != MAGICNUM)) return STAT_CANONICAL_MACHINE_ASSERTION_FAILURE; + if ((cm.gmx.magic_start != MAGICNUM) || (cm.gmx.magic_end != MAGICNUM)) return STAT_CANONICAL_MACHINE_ASSERTION_FAILURE; + if ((arc.magic_start != MAGICNUM) || (arc.magic_end != MAGICNUM)) return STAT_CANONICAL_MACHINE_ASSERTION_FAILURE; + + return STAT_OK; } + /* * cm_soft_alarm() - alarm state; send an exception report and stop processing input - * cm_clear() - clear soft alarm + * cm_clear() - clear soft alarm * cm_hard_alarm() - alarm state; send an exception report and shut down machine */ - -stat_t cm_soft_alarm(stat_t status) -{ - rpt_exception(status); // send alarm message - cm.machine_state = MACHINE_ALARM; - return (status); // NB: More efficient than inlining rpt_exception() call. +stat_t cm_soft_alarm(stat_t status) { + rpt_exception(status); // send alarm message + cm.machine_state = MACHINE_ALARM; + return status; // NB: More efficient than inlining rpt_exception() call. } -stat_t cm_clear(nvObj_t *nv) // clear soft alarm -{ - if (cm.cycle_state == CYCLE_OFF) { - cm.machine_state = MACHINE_PROGRAM_STOP; - } else { - cm.machine_state = MACHINE_CYCLE; - } - return (STAT_OK); -} -stat_t cm_hard_alarm(stat_t status) -{ - // stop the motors and the spindle - stepper_init(); // hard stop - cm_spindle_control(SPINDLE_OFF); +stat_t cm_clear(nvObj_t *nv) { // clear soft alarm + if (cm.cycle_state == CYCLE_OFF) + cm.machine_state = MACHINE_PROGRAM_STOP; + else cm.machine_state = MACHINE_CYCLE; + + return STAT_OK; +} - // disable all MCode functions -// gpio_set_bit_off(SPINDLE_BIT); //++++ this current stuff is temporary -// gpio_set_bit_off(SPINDLE_DIR); -// gpio_set_bit_off(SPINDLE_PWM); -// gpio_set_bit_off(MIST_COOLANT_BIT); //++++ replace with exec function -// gpio_set_bit_off(FLOOD_COOLANT_BIT); //++++ replace with exec function - rpt_exception(status); // send shutdown message - cm.machine_state = MACHINE_SHUTDOWN; - return (status); +stat_t cm_hard_alarm(stat_t status) { + // stop the motors and the spindle + stepper_init(); // hard stop + cm_spindle_control(SPINDLE_OFF); + rpt_exception(status); // send shutdown message + cm.machine_state = MACHINE_SHUTDOWN; + return status; } /************************** @@ -609,57 +562,55 @@ stat_t cm_hard_alarm(stat_t status) /************************************************************************** * 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) + * 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. + * These functions assume input validation occurred upstream. */ - -stat_t cm_select_plane(uint8_t plane) -{ - cm.gm.select_plane = plane; - return (STAT_OK); +stat_t cm_select_plane(uint8_t plane) { + cm.gm.select_plane = plane; + return STAT_OK; } -stat_t cm_set_units_mode(uint8_t mode) -{ - cm.gm.units_mode = mode; // 0 = inches, 1 = mm. - return(STAT_OK); + +stat_t cm_set_units_mode(uint8_t mode) { + cm.gm.units_mode = mode; // 0 = inches, 1 = mm. + return(STAT_OK); } -stat_t cm_set_distance_mode(uint8_t mode) -{ - cm.gm.distance_mode = mode; // 0 = absolute mode, 1 = incremental - return (STAT_OK); + +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) * - * This function applies the offset to the GM model but does not persist the offsets - * during the Gcode cycle. The persist flag is used to persist offsets once the cycle - * has ended. You can also use $g54x - $g59c config functions to change offsets. + * This function applies the offset to the GM model but does not persist the offsets + * during the Gcode cycle. The persist flag is used to persist offsets once the cycle + * has ended. You can also use $g54x - $g59c config functions to change offsets. * - * It also does not reset the work_offsets which may be accomplished by calling - * cm_set_work_offsets() immediately afterwards. + * It also does not reset the work_offsets which may be accomplished by calling + * cm_set_work_offsets() immediately afterwards. */ +stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]) { + if ((coord_system < G54) || (coord_system > COORD_SYSTEM_MAX)) // you can't set G53 + return STAT_INPUT_VALUE_RANGE_ERROR; + + for (uint8_t axis = AXIS_X; axis < AXES; axis++) + if (fp_TRUE(flag[axis])) { + cm.offset[coord_system][axis] = _to_millimeters(offset[axis]); + cm.deferred_write_flag = true; // persist offsets once machining cycle is over + } -stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]) -{ - if ((coord_system < G54) || (coord_system > COORD_SYSTEM_MAX)) { // you can't set G53 - return (STAT_INPUT_VALUE_RANGE_ERROR); - } - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (fp_TRUE(flag[axis])) { - cm.offset[coord_system][axis] = _to_millimeters(offset[axis]); - cm.deferred_write_flag = true; // persist offsets once machining cycle is over - } - } - return (STAT_OK); + return STAT_OK; } + /****************************************************************************************** * Representation functions that affect gcode model and are queued to planner (synchronous) */ @@ -667,148 +618,150 @@ stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]) * cm_set_coord_system() - G54-G59 * _exec_offset() - callback from planner */ -stat_t cm_set_coord_system(uint8_t coord_system) -{ - cm.gm.coord_system = coord_system; +stat_t cm_set_coord_system(uint8_t coord_system) { + cm.gm.coord_system = coord_system; - float value[AXES] = { (float)coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element - mp_queue_command(_exec_offset, value, value); // second vector (flags) is not used, so fake it - return (STAT_OK); + float value[AXES] = { (float)coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element + mp_queue_command(_exec_offset, value, value); // second vector (flags) is not used, so fake it + return STAT_OK; } -static void _exec_offset(float *value, float *flag) -{ - uint8_t coord_system = ((uint8_t)value[0]); // coordinate system is passed in value[0] element - 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); - } - mp_set_runtime_work_offset(offsets); - cm_set_work_offsets(MODEL); // set work offsets in the Gcode model + +static void _exec_offset(float *value, float *flag) { + uint8_t coord_system = ((uint8_t)value[0]); // coordinate system is passed in value[0] element + 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); + + mp_set_runtime_work_offset(offsets); + 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 * - * 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 !!!!! + * !!!!! 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 - cm.gmx.position[axis] = position; - cm.gm.target[axis] = position; - mp_set_planner_position(axis, position); - mp_set_runtime_position(axis, position); - mp_set_steps_to_runtime_position(); +void cm_set_position(uint8_t axis, float position) { + // TODO: Interlock involving runtime_busy test + cm.gmx.position[axis] = position; + cm.gm.target[axis] = position; + mp_set_planner_position(axis, position); + mp_set_runtime_position(axis, position); + mp_set_steps_to_runtime_position(); } + /*** G28.3 functions and support *** * * cm_set_absolute_origin() - G28.3 - model, planner and queue to runtime * _exec_absolute_origin() - callback from planner * - * 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). + * 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). * - * This is a 2 step process. The model and planner contexts are set immediately, the runtime - * command is queued and synchronized with the planner queue. This includes the runtime position - * and the step recording done by the encoders. At that point any axis that is set is also marked - * as homed. + * This is a 2 step process. The model and planner contexts are set immediately, the runtime + * command is queued and synchronized with the planner queue. This includes the runtime position + * and the step recording done by the encoders. At that point any axis that is set is also marked + * as homed. */ +stat_t cm_set_absolute_origin(float origin[], float flag[]) { + float value[AXES]; + + for (uint8_t axis = AXIS_X; axis < AXES; axis++) + if (fp_TRUE(flag[axis])) { + value[axis] = _to_millimeters(origin[axis]); + cm.gmx.position[axis] = value[axis]; // set model position + cm.gm.target[axis] = value[axis]; // reset model target + mp_set_planner_position(axis, value[axis]); // set mm position + } -stat_t cm_set_absolute_origin(float origin[], float flag[]) -{ - float value[AXES]; - - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (fp_TRUE(flag[axis])) { - value[axis] = _to_millimeters(origin[axis]); - cm.gmx.position[axis] = value[axis]; // set model position - cm.gm.target[axis] = value[axis]; // reset model target - mp_set_planner_position(axis, value[axis]); // set mm position - } - } - mp_queue_command(_exec_absolute_origin, value, flag); - return (STAT_OK); -} - -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 - } - } - mp_set_steps_to_runtime_position(); + mp_queue_command(_exec_absolute_origin, value, flag); + + return STAT_OK; } + +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 + } + + 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 + * 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 * http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3 */ -stat_t cm_set_origin_offsets(float offset[], float flag[]) -{ - // set offsets in the Gcode model extended context - cm.gmx.origin_offset_enable = 1; - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - if (fp_TRUE(flag[axis])) { - 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 - return (STAT_OK); -} - -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 }; - mp_queue_command(_exec_offset, value, value); - return (STAT_OK); -} - -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 }; - mp_queue_command(_exec_offset, value, value); - return (STAT_OK); -} - -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 }; - mp_queue_command(_exec_offset, value, value); - return (STAT_OK); +stat_t cm_set_origin_offsets(float offset[], float flag[]) { + // set offsets in the Gcode model extended context + cm.gmx.origin_offset_enable = 1; + for (uint8_t axis = AXIS_X; axis < AXES; axis++) + if (fp_TRUE(flag[axis])) + 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 + + return STAT_OK; } + +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 }; + mp_queue_command(_exec_offset, value, value); + + return STAT_OK; +} + + +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 }; + mp_queue_command(_exec_offset, value, value); + + return STAT_OK; +} + + +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 }; + mp_queue_command(_exec_offset, value, value); + + return STAT_OK; +} + + /***************************** * Free Space Motion (4.3.4) * *****************************/ @@ -816,60 +769,59 @@ stat_t cm_resume_origin_offsets() * cm_straight_traverse() - G0 linear rapid */ -stat_t cm_straight_traverse(float target[], float flags[]) -{ - cm.gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; - cm_set_model_target(target, flags); +stat_t cm_straight_traverse(float target[], float flags[]) { + cm.gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; + cm_set_model_target(target, flags); - // test soft limits - stat_t status = cm_test_soft_limits(cm.gm.target); - if (status != STAT_OK) return (cm_soft_alarm(status)); + // test soft limits + stat_t status = cm_test_soft_limits(cm.gm.target); + 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_finalize_move(); - return (STAT_OK); + // 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_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 */ - -stat_t cm_set_g28_position(void) -{ - copy_vector(cm.gmx.g28_position, cm.gmx.position); - return (STAT_OK); +stat_t cm_set_g28_position() { + copy_vector(cm.gmx.g28_position, cm.gmx.position); + return STAT_OK; } -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 - float f[] = {1,1,1,1,1,1}; - return(cm_straight_traverse(cm.gmx.g28_position, f));// execute actual stored move + +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 + float f[] = {1,1,1,1,1,1}; + return cm_straight_traverse(cm.gmx.g28_position, f);// execute actual stored move } -stat_t cm_set_g30_position(void) -{ - copy_vector(cm.gmx.g30_position, cm.gmx.position); - return (STAT_OK); + +stat_t cm_set_g30_position() { + copy_vector(cm.gmx.g30_position, cm.gmx.position); + return STAT_OK; } -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 - float f[] = {1,1,1,1,1,1}; - return(cm_straight_traverse(cm.gmx.g30_position, f));// execute actual stored move + +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 + float f[] = {1,1,1,1,1,1}; + return cm_straight_traverse(cm.gmx.g30_position, f);// execute actual stored move } + /******************************** * Machining Attributes (4.3.5) * ********************************/ @@ -878,41 +830,37 @@ stat_t cm_goto_g30_position(float target[], float flags[]) * * 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 (NB: active for this gcode block only) + else cm.gm.feed_rate = _to_millimeters(feed_rate); -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 (NB: active for this gcode block only) - } else { - cm.gm.feed_rate = _to_millimeters(feed_rate); - } - return (STAT_OK); + return STAT_OK; } + /* * cm_set_feed_rate_mode() - 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; - return (STAT_OK); +stat_t cm_set_feed_rate_mode(uint8_t mode) { + cm.gm.feed_rate_mode = mode; + return STAT_OK; } + /* * cm_set_path_control() - G61, G61.1, G64 (affects MODEL only) */ - -stat_t cm_set_path_control(uint8_t mode) -{ - cm.gm.path_control = mode; - return (STAT_OK); +stat_t cm_set_path_control(uint8_t mode) { + cm.gm.path_control = mode; + return STAT_OK; } + /******************************* * Machining Functions (4.3.6) * *******************************/ @@ -923,37 +871,37 @@ stat_t cm_set_path_control(uint8_t mode) /* * cm_dwell() - G4, P parameter (seconds) */ -stat_t cm_dwell(float seconds) -{ - cm.gm.parameter = seconds; - mp_dwell(seconds); - return (STAT_OK); +stat_t cm_dwell(float seconds) { + cm.gm.parameter = seconds; + mp_dwell(seconds); + return STAT_OK; } + /* * cm_straight_feed() - 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))) { - return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED); - } - cm.gm.motion_mode = MOTION_MODE_STRAIGHT_FEED; - cm_set_model_target(target, flags); - - // test soft limits - stat_t status = cm_test_soft_limits(cm.gm.target); - 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 - status = mp_aline(&cm.gm); // send the move to the planner - cm_finalize_move(); - return (status); +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))) + return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; + + cm.gm.motion_mode = MOTION_MODE_STRAIGHT_FEED; + cm_set_model_target(target, flags); + + // test soft limits + stat_t status = cm_test_soft_limits(cm.gm.target); + 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 + status = mp_aline(&cm.gm); // send the move to the planner + cm_finalize_move(); + return status; } + /***************************** * Spindle Functions (4.3.7) * *****************************/ @@ -963,39 +911,39 @@ stat_t cm_straight_feed(float target[], float flags[]) * Tool Functions (4.3.8) * **************************/ /* - * cm_select_tool() - T parameter - * _exec_select_tool() - execution callback + * 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 + * cm_change_tool() - M6 (This might become a complete tool change cycle) + * _exec_change_tool() - execution callback * * 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 */ -stat_t cm_select_tool(uint8_t tool_select) -{ - float value[AXES] = { (float)tool_select,0,0,0,0,0 }; - mp_queue_command(_exec_select_tool, value, value); - return (STAT_OK); +stat_t cm_select_tool(uint8_t tool_select) { + float value[AXES] = { (float)tool_select,0,0,0,0,0 }; + mp_queue_command(_exec_select_tool, value, value); + return STAT_OK; } -static void _exec_select_tool(float *value, float *flag) -{ - cm.gm.tool_select = (uint8_t)value[0]; + +static void _exec_select_tool(float *value, float *flag) { + cm.gm.tool_select = (uint8_t)value[0]; } -stat_t cm_change_tool(uint8_t tool_change) -{ - float value[AXES] = { (float)cm.gm.tool_select,0,0,0,0,0 }; - mp_queue_command(_exec_change_tool, value, value); - return (STAT_OK); + +stat_t cm_change_tool(uint8_t tool_change) { + float value[AXES] = { (float)cm.gm.tool_select,0,0,0,0,0 }; + mp_queue_command(_exec_change_tool, value, value); + return STAT_OK; } -static void _exec_change_tool(float *value, float *flag) -{ - cm.gm.tool = (uint8_t)value[0]; + +static void _exec_change_tool(float *value, float *flag) { + cm.gm.tool = (uint8_t)value[0]; } + /*********************************** * Miscellaneous Functions (4.3.9) * ***********************************/ @@ -1003,41 +951,41 @@ static void _exec_change_tool(float *value, float *flag) * cm_mist_coolant_control() - M7 * cm_flood_coolant_control() - M8, M9 */ - -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); - return (STAT_OK); +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); + return STAT_OK; } -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 - gpio_set_bit_off(MIST_COOLANT_BIT); // else + +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 + gpio_set_bit_off(MIST_COOLANT_BIT); // else } -stat_t cm_flood_coolant_control(uint8_t flood_coolant) -{ - float value[AXES] = { (float)flood_coolant,0,0,0,0,0 }; - mp_queue_command(_exec_flood_coolant_control, value, value); - return (STAT_OK); + +stat_t cm_flood_coolant_control(uint8_t flood_coolant) { + float value[AXES] = { (float)flood_coolant,0,0,0,0,0 }; + mp_queue_command(_exec_flood_coolant_control, value, value); + return STAT_OK; } -static void _exec_flood_coolant_control(float *value, float *flag) -{ - cm.gm.flood_coolant = (uint8_t)value[0]; - if (cm.gm.flood_coolant == true) { - gpio_set_bit_on(FLOOD_COOLANT_BIT); - } 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 - } + +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); + 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 + } } + /* * cm_override_enables() - M48, M49 * cm_feed_rate_override_enable() - M50 @@ -1047,83 +995,76 @@ static void _exec_flood_coolant_control(float *value, float *flag) * 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 -{ - cm.gmx.feed_rate_override_enable = flag; - cm.gmx.traverse_override_enable = flag; - cm.gmx.spindle_override_enable = flag; - return (STAT_OK); +stat_t cm_override_enables(uint8_t flag) { // M48, M49 + cm.gmx.feed_rate_override_enable = flag; + cm.gmx.traverse_override_enable = flag; + cm.gmx.spindle_override_enable = flag; + return STAT_OK; } -stat_t cm_feed_rate_override_enable(uint8_t flag) // M50 -{ - 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; - } - return (STAT_OK); + +stat_t cm_feed_rate_override_enable(uint8_t flag) { // M50 + 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; + + return STAT_OK; } -stat_t cm_feed_rate_override_factor(uint8_t flag) // M50.1 -{ - cm.gmx.feed_rate_override_enable = flag; - cm.gmx.feed_rate_override_factor = cm.gn.parameter; -// mp_feed_rate_override(flag, cm.gn.parameter); // replan the queue for new feed rate - return (STAT_OK); + +stat_t cm_feed_rate_override_factor(uint8_t flag) { // M50.1 + 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 -{ - if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter)) { - cm.gmx.traverse_override_enable = false; - } else { - cm.gmx.traverse_override_enable = true; - } - return (STAT_OK); + +stat_t cm_traverse_override_enable(uint8_t flag) { // M50.2 + if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter)) + cm.gmx.traverse_override_enable = false; + else cm.gmx.traverse_override_enable = true; + + return STAT_OK; } -stat_t cm_traverse_override_factor(uint8_t flag) // M51 -{ - cm.gmx.traverse_override_enable = flag; - cm.gmx.traverse_override_factor = cm.gn.parameter; -// mp_feed_rate_override(flag, cm.gn.parameter); // replan the queue for new feed rate - return (STAT_OK); + +stat_t cm_traverse_override_factor(uint8_t flag) { // M51 + 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 -{ - if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter)) { - cm.gmx.spindle_override_enable = false; - } else { - cm.gmx.spindle_override_enable = true; - } - return (STAT_OK); + +stat_t cm_spindle_override_enable(uint8_t flag) { // M51.1 + if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter)) + cm.gmx.spindle_override_enable = false; + else cm.gmx.spindle_override_enable = true; + + return STAT_OK; } -stat_t cm_spindle_override_factor(uint8_t flag) // M50.1 -{ - cm.gmx.spindle_override_enable = flag; - cm.gmx.spindle_override_factor = cm.gn.parameter; -// change spindle speed - return (STAT_OK); + +stat_t cm_spindle_override_factor(uint8_t flag) { // M50.1 + cm.gmx.spindle_override_enable = flag; + cm.gmx.spindle_override_factor = cm.gn.parameter; + + return STAT_OK; } + /* * cm_message() - queue a RAM string as a message in the response (unconditionally) * - * Note: If you need to post a FLASH string use pstr2str to convert it to a RAM string + * Note: If you need to post a FLASH string use pstr2str to convert it to a RAM string */ - -void cm_message(char_t *message) -{ - nv_add_string((const char_t *)"msg", message); // add message to the response object +void cm_message(char_t *message) { + nv_add_string((const char_t *)"msg", message); // add message to the response object } + /****************************** * Program Functions (4.3.10) * ******************************/ @@ -1131,14 +1072,14 @@ void cm_message(char_t *message) * 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 + * _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 @@ -1158,195 +1099,187 @@ void cm_message(char_t *message) * 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 + * 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 */ -void cm_request_feedhold(void) { cm.feedhold_requested = true; } -void cm_request_queue_flush(void) { cm.queue_flush_requested = true; } -void cm_request_cycle_start(void) { cm.cycle_start_requested = true; } - -stat_t cm_feedhold_sequencing_callback() -{ - if (cm.feedhold_requested == true) { - if ((cm.motion_state == MOTION_RUN) && (cm.hold_state == FEEDHOLD_OFF)) { - cm_set_motion_state(MOTION_HOLD); - cm.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution - } - cm.feedhold_requested = false; - } - if (cm.queue_flush_requested == true) { - if (((cm.motion_state == MOTION_STOP) || - ((cm.motion_state == MOTION_HOLD) && (cm.hold_state == FEEDHOLD_HOLD))) && - !cm_get_runtime_busy()) { - cm.queue_flush_requested = false; - cm_queue_flush(); - } - } - 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) { - cm.cycle_start_requested = false; - cm.hold_state = FEEDHOLD_END_HOLD; - cm_cycle_start(); - mp_end_hold(); - } - return (STAT_OK); -} - -stat_t cm_queue_flush() -{ - if (cm_get_runtime_busy() == true) - return (STAT_COMMAND_NOT_ACCEPTED); - - xio_reset_usb_rx_buffers(); // flush serial queues - mp_flush_planner(); // flush planner queue - qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available - rx_request_rx_report(); - - // 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 - } - float value[AXES] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - _exec_program_finalize(value, value); // finalize now, not later - return (STAT_OK); +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;} + + +stat_t cm_feedhold_sequencing_callback() { + if (cm.feedhold_requested == true) { + if ((cm.motion_state == MOTION_RUN) && (cm.hold_state == FEEDHOLD_OFF)) { + cm_set_motion_state(MOTION_HOLD); + cm.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution + } + cm.feedhold_requested = false; + } + + if (cm.queue_flush_requested == true) { + if (((cm.motion_state == MOTION_STOP) || + ((cm.motion_state == MOTION_HOLD) && (cm.hold_state == FEEDHOLD_HOLD))) && + !cm_get_runtime_busy()) { + cm.queue_flush_requested = false; + cm_queue_flush(); + } + } + + 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) { + cm.cycle_start_requested = false; + cm.hold_state = FEEDHOLD_END_HOLD; + cm_cycle_start(); + mp_end_hold(); + } + + return STAT_OK; +} + + +stat_t cm_queue_flush() { + if (cm_get_runtime_busy() == true) return STAT_COMMAND_NOT_ACCEPTED; + + xio_reset_usb_rx_buffers(); // flush serial queues + mp_flush_planner(); // flush planner queue + qr_request_queue_report(0); // request a queue report, since we've changed the number of buffers available + rx_request_rx_report(); + + // 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 + + float value[AXES] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; + _exec_program_finalize(value, value); // finalize now, not later + + return STAT_OK; } + /* * Program and cycle state functions * - * _exec_program_finalize() - helper + * _exec_program_finalize() - helper * cm_cycle_start() * cm_cycle_end() - * cm_program_stop() - M0 - * cm_optional_program_stop() - M1 - * cm_program_end() - M2, M30 + * cm_program_stop() - M0 + * cm_optional_program_stop() - M1 + * cm_program_end() - M2, M30 * * 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) - * 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) - * 5. Feed and speed overrides are set to ON (like M48) - * 6. Cutter compensation is turned off (like G40) - * 7. The spindle is stopped (like M5) - * 8. The current motion mode is set to G_1 (like G1) - * 9. Coolant is turned off (like M9) + * 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) + * 5. Feed and speed overrides are set to ON (like M48) + * 6. Cutter compensation is turned off (like G40) + * 7. The spindle is stopped (like M5) + * 8. The current motion mode is set to G_1 (like G1) + * 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) - * Set default coordinate system (uses $gco, not 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 - * 6. Not implemented - * 7. The spindle is stopped (like M5) - * 8. Motion mode is canceled like G80 (not set to G1) - * 9. Coolant is turned off (like M9) - * + Default INCHES or MM units mode is restored ($gun) + * 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) + * 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 + * 6. Not implemented + * 7. The spindle is stopped (like M5) + * 8. Motion mode is canceled like G80 (not set to G1) + * 9. Coolant is turned off (like M9) + * + Default INCHES or MM units mode is restored ($gun) */ +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. -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.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_suspend_origin_offsets(); // G92.2 - as per Kramer - 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_set_units_mode(cm.units_mode); // reset to default units mode +++ REMOVED +++ - cm_spindle_control(SPINDLE_OFF); // M5 - cm_flood_coolant_control(false); // M9 - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94 - // cm_set_motion_mode(MOTION_MODE_STRAIGHT_FEED); // NIST specifies G1, but we cancel motion mode. Safer. - cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); - } - sr_request_status_report(SR_IMMEDIATE_REQUEST); // request a final status report (not unfiltered) -} - -void cm_cycle_start() -{ - cm.machine_state = MACHINE_CYCLE; - if (cm.cycle_state == CYCLE_OFF) { // don't (re)start homing, probe or other canned cycles - cm.cycle_state = CYCLE_MACHINING; - qr_init_queue_report(); // clear queue reporting buffer counts - } -} - -void cm_cycle_end() -{ - if (cm.cycle_state != CYCLE_OFF) { - float value[AXES] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - _exec_program_finalize(value, value); - } -} - -void cm_program_stop() -{ - float value[AXES] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - mp_queue_command(_exec_program_finalize, value, value); -} - -void cm_optional_program_stop() -{ - float value[AXES] = { (float)MACHINE_PROGRAM_STOP, 0,0,0,0,0 }; - mp_queue_command(_exec_program_finalize, value, value); -} - -void cm_program_end() -{ - float value[AXES] = { (float)MACHINE_PROGRAM_END, 0,0,0,0,0 }; - mp_queue_command(_exec_program_finalize, value, value); -} - -/************************************** - * END OF CANONICAL MACHINE FUNCTIONS * - **************************************/ + 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_set_distance_mode(cm.distance_mode); + cm_spindle_control(SPINDLE_OFF); // M5 + cm_flood_coolant_control(false); // M9 + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94 + cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); + } + + sr_request_status_report(SR_IMMEDIATE_REQUEST); // request a final status report (not unfiltered) +} + + +void cm_cycle_start() { + cm.machine_state = MACHINE_CYCLE; + + if (cm.cycle_state == CYCLE_OFF) { // don't (re)start homing, probe or other canned cycles + cm.cycle_state = CYCLE_MACHINING; + qr_init_queue_report(); // clear queue reporting buffer counts + } +} + + +void cm_cycle_end() { + if (cm.cycle_state != CYCLE_OFF) { + float value[AXES] = {(float)MACHINE_PROGRAM_STOP, 0,0,0,0,0}; + _exec_program_finalize(value, value); + } +} + + +void cm_program_stop() { + float value[AXES] = {(float)MACHINE_PROGRAM_STOP, 0,0,0,0,0}; + mp_queue_command(_exec_program_finalize, value, value); +} + + +void cm_optional_program_stop() { + float value[AXES] = {(float)MACHINE_PROGRAM_STOP, 0,0,0,0,0}; + mp_queue_command(_exec_program_finalize, value, value); +} + + +void cm_program_end() { + float value[AXES] = {(float)MACHINE_PROGRAM_END, 0,0,0,0,0}; + mp_queue_command(_exec_program_finalize, value, value); +} -/*********************************************************************************** - * CONFIGURATION AND INTERFACE FUNCTIONS - * Functions to get and set variables from the cfgArray table - * These functions are not part of the NIST defined functions - ***********************************************************************************/ // Strings for writing settings as nvObj string values // Ref: http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=120881&start=0 - #ifdef __TEXT_MODE -static const char msg_units0[] PROGMEM = " in"; // used by generic print functions +static const char msg_units0[] PROGMEM = " in"; // used by generic print functions static const char msg_units1[] PROGMEM = " mm"; static const char msg_units2[] PROGMEM = " deg"; static const char *const msg_units[] PROGMEM = { msg_units0, msg_units1, msg_units2 }; @@ -1362,7 +1295,7 @@ static const char msg_g20[] PROGMEM = "G20 - inches mode"; static const char msg_g21[] PROGMEM = "G21 - millimeter mode"; static const char *const msg_unit[] PROGMEM = { msg_g20, msg_g21 }; -static const char msg_stat0[] PROGMEM = "Initializing"; // combined state (stat) uses this array +static const char msg_stat0[] PROGMEM = "Initializing"; // combined state (stat) uses this array static const char msg_stat1[] PROGMEM = "Ready"; static const char msg_stat2[] PROGMEM = "Alarm"; static const char msg_stat3[] PROGMEM = "Stop"; @@ -1375,8 +1308,8 @@ static const char msg_stat9[] PROGMEM = "Homing"; static const char msg_stat10[] PROGMEM = "Jog"; static const char msg_stat11[] PROGMEM = "Shutdown"; static const char *const msg_stat[] PROGMEM = { msg_stat0, msg_stat1, msg_stat2, msg_stat3, - msg_stat4, msg_stat5, msg_stat6, msg_stat7, - msg_stat8, msg_stat9, msg_stat10, msg_stat11 }; + msg_stat4, msg_stat5, msg_stat6, msg_stat7, + msg_stat8, msg_stat9, msg_stat10, msg_stat11 }; static const char msg_macs0[] PROGMEM = "Initializing"; static const char msg_macs1[] PROGMEM = "Ready"; @@ -1386,7 +1319,7 @@ static const char msg_macs4[] PROGMEM = "End"; static const char msg_macs5[] PROGMEM = "Cycle"; static const char msg_macs6[] PROGMEM = "Shutdown"; static const char *const msg_macs[] PROGMEM = { msg_macs0, msg_macs1, msg_macs2, msg_macs3, - msg_macs4, msg_macs5, msg_macs6 }; + msg_macs4, msg_macs5, msg_macs6 }; static const char msg_cycs0[] PROGMEM = "Off"; static const char msg_cycs1[] PROGMEM = "Machining"; @@ -1407,7 +1340,7 @@ static const char msg_hold3[] PROGMEM = "Decel"; static const char msg_hold4[] PROGMEM = "Hold"; static const char msg_hold5[] PROGMEM = "End Hold"; static const char *const msg_hold[] PROGMEM = { msg_hold0, msg_hold1, msg_hold2, msg_hold3, - msg_hold4, msg_hold5 }; + msg_hold4, msg_hold5 }; static const char msg_home0[] PROGMEM = "Not Homed"; static const char msg_home1[] PROGMEM = "Homed"; @@ -1451,60 +1384,60 @@ static const char *const msg_frmo[] PROGMEM = { msg_g93, msg_g94, msg_g95 }; #else -#define msg_units NULL -#define msg_unit NULL -#define msg_stat NULL -#define msg_macs NULL -#define msg_cycs NULL -#define msg_mots NULL -#define msg_hold NULL -#define msg_home NULL -#define msg_coor NULL -#define msg_momo NULL -#define msg_plan NULL -#define msg_path NULL -#define msg_dist NULL -#define msg_frmo NULL -#define msg_am NULL +#define msg_units 0 +#define msg_unit 0 +#define msg_stat 0 +#define msg_macs 0 +#define msg_cycs 0 +#define msg_mots 0 +#define msg_hold 0 +#define msg_home 0 +#define msg_coor 0 +#define msg_momo 0 +#define msg_plan 0 +#define msg_path 0 +#define msg_dist 0 +#define msg_frmo 0 +#define msg_am 0 #endif // __TEXT_MODE /***** AXIS HELPERS ***************************************************************** * cm_get_axis_char() - return ASCII char for axis given the axis number - * _get_axis() - return axis number or -1 if NA - * _get_axis_type() - return 0 -f axis is linear, 1 if rotary, -1 if NA + * _get_axis() - return axis number or -1 if NA + * _get_axis_type() - return 0 -f axis is linear, 1 if rotary, -1 if NA */ -char_t cm_get_axis_char(const int8_t axis) -{ - char_t axis_char[] = "XYZABC"; - if ((axis < 0) || (axis > AXES)) return (' '); - return (axis_char[axis]); +char_t cm_get_axis_char(const int8_t axis) { + char_t axis_char[] = "XYZABC"; + if ((axis < 0) || (axis > AXES)) return ' '; + return axis_char[axis]; } -static int8_t _get_axis(const index_t index) -{ - char_t *ptr; - char_t tmp[TOKEN_LEN+1]; - char_t axes[] = {"xyzabc"}; - strncpy_P(tmp, cfgArray[index].token, TOKEN_LEN); // kind of a hack. Looks for an axis - if ((ptr = strchr(axes, tmp[0])) == NULL) { // character in the 0 and 3 positions - if ((ptr = strchr(axes, tmp[3])) == NULL) { // to accommodate 'xam' and 'g54x' styles - return (-1); - } - } - return (ptr - axes); +static int8_t _get_axis(const index_t index) { + char_t *ptr; + char_t tmp[TOKEN_LEN+1]; + char_t axes[] = {"xyzabc"}; + + strncpy_P(tmp, cfgArray[index].token, TOKEN_LEN); // kind of a hack. Looks for an axis + if ((ptr = strchr(axes, tmp[0])) == 0) { // character in the 0 and 3 positions + if ((ptr = strchr(axes, tmp[3])) == 0) // to accommodate 'xam' and 'g54x' styles + return -1; + } + + return ptr - axes; } -static int8_t _get_axis_type(const index_t index) -{ - int8_t axis = _get_axis(index); - if (axis >= AXIS_A) return (1); - if (axis == -1) return (-1); - return (0); + +static int8_t _get_axis_type(const index_t index) { + int8_t axis = _get_axis(index); + if (axis >= AXIS_A) return 1; + if (axis == -1) return -1; + return 0; } + /**** Functions called directly from cfgArray table - mostly wrappers **** * _get_msg_helper() - helper to get string values * @@ -1536,162 +1469,157 @@ static int8_t _get_axis_type(const index_t index) * cm_print_coor()- print coordinate offsets with linear units * cm_print_corr()- print coordinate offsets with rotary units */ +stat_t _get_msg_helper(nvObj_t *nv, const char *const msg_array[], uint8_t value) { + nv->value = (float)value; + nv->valuetype = TYPE_INTEGER; + return(nv_copy_string(nv, (const char_t *)GET_TEXT_ITEM(msg_array, value))); +} + + +stat_t cm_get_stat(nvObj_t *nv) {return(_get_msg_helper(nv, msg_stat, cm_get_combined_state()));} +stat_t cm_get_macs(nvObj_t *nv) {return(_get_msg_helper(nv, msg_macs, cm_get_machine_state()));} +stat_t cm_get_cycs(nvObj_t *nv) {return(_get_msg_helper(nv, msg_cycs, cm_get_cycle_state()));} +stat_t cm_get_mots(nvObj_t *nv) {return(_get_msg_helper(nv, msg_mots, cm_get_motion_state()));} +stat_t cm_get_hold(nvObj_t *nv) {return(_get_msg_helper(nv, msg_hold, cm_get_hold_state()));} +stat_t cm_get_home(nvObj_t *nv) {return(_get_msg_helper(nv, msg_home, cm_get_homing_state()));} + +stat_t cm_get_unit(nvObj_t *nv) {return(_get_msg_helper(nv, msg_unit, cm_get_units_mode(ACTIVE_MODEL)));} +stat_t cm_get_coor(nvObj_t *nv) {return(_get_msg_helper(nv, msg_coor, cm_get_coord_system(ACTIVE_MODEL)));} +stat_t cm_get_momo(nvObj_t *nv) {return(_get_msg_helper(nv, msg_momo, cm_get_motion_mode(ACTIVE_MODEL)));} +stat_t cm_get_plan(nvObj_t *nv) {return(_get_msg_helper(nv, msg_plan, cm_get_select_plane(ACTIVE_MODEL)));} +stat_t cm_get_path(nvObj_t *nv) {return(_get_msg_helper(nv, msg_path, cm_get_path_control(ACTIVE_MODEL)));} +stat_t cm_get_dist(nvObj_t *nv) {return(_get_msg_helper(nv, msg_dist, cm_get_distance_mode(ACTIVE_MODEL)));} +stat_t cm_get_frmo(nvObj_t *nv) {return(_get_msg_helper(nv, msg_frmo, cm_get_feed_rate_mode(ACTIVE_MODEL)));} + + +stat_t cm_get_toolv(nvObj_t *nv) { + nv->value = (float)cm_get_tool(ACTIVE_MODEL); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; +} + + +stat_t cm_get_mline(nvObj_t *nv) { + nv->value = (float)cm_get_linenum(MODEL); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; +} + + +stat_t cm_get_line(nvObj_t *nv) { + nv->value = (float)cm_get_linenum(ACTIVE_MODEL); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; +} + -stat_t _get_msg_helper(nvObj_t *nv, const char *const msg_array[], uint8_t value) -{ - nv->value = (float)value; - nv->valuetype = TYPE_INTEGER; - return(nv_copy_string(nv, (const char_t *)GET_TEXT_ITEM(msg_array, value))); -} - -stat_t cm_get_stat(nvObj_t *nv) { return(_get_msg_helper(nv, msg_stat, cm_get_combined_state()));} -stat_t cm_get_macs(nvObj_t *nv) { return(_get_msg_helper(nv, msg_macs, cm_get_machine_state()));} -stat_t cm_get_cycs(nvObj_t *nv) { return(_get_msg_helper(nv, msg_cycs, cm_get_cycle_state()));} -stat_t cm_get_mots(nvObj_t *nv) { return(_get_msg_helper(nv, msg_mots, cm_get_motion_state()));} -stat_t cm_get_hold(nvObj_t *nv) { return(_get_msg_helper(nv, msg_hold, cm_get_hold_state()));} -stat_t cm_get_home(nvObj_t *nv) { return(_get_msg_helper(nv, msg_home, cm_get_homing_state()));} - -stat_t cm_get_unit(nvObj_t *nv) { return(_get_msg_helper(nv, msg_unit, cm_get_units_mode(ACTIVE_MODEL)));} -stat_t cm_get_coor(nvObj_t *nv) { return(_get_msg_helper(nv, msg_coor, cm_get_coord_system(ACTIVE_MODEL)));} -stat_t cm_get_momo(nvObj_t *nv) { return(_get_msg_helper(nv, msg_momo, cm_get_motion_mode(ACTIVE_MODEL)));} -stat_t cm_get_plan(nvObj_t *nv) { return(_get_msg_helper(nv, msg_plan, cm_get_select_plane(ACTIVE_MODEL)));} -stat_t cm_get_path(nvObj_t *nv) { return(_get_msg_helper(nv, msg_path, cm_get_path_control(ACTIVE_MODEL)));} -stat_t cm_get_dist(nvObj_t *nv) { return(_get_msg_helper(nv, msg_dist, cm_get_distance_mode(ACTIVE_MODEL)));} -stat_t cm_get_frmo(nvObj_t *nv) { return(_get_msg_helper(nv, msg_frmo, cm_get_feed_rate_mode(ACTIVE_MODEL)));} - -stat_t cm_get_toolv(nvObj_t *nv) -{ - nv->value = (float)cm_get_tool(ACTIVE_MODEL); - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); -} - -stat_t cm_get_mline(nvObj_t *nv) -{ - nv->value = (float)cm_get_linenum(MODEL); - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); -} - -stat_t cm_get_line(nvObj_t *nv) -{ - nv->value = (float)cm_get_linenum(ACTIVE_MODEL); - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); -} - -stat_t cm_get_vel(nvObj_t *nv) -{ - if (cm_get_motion_state() == MOTION_STOP) { - nv->value = 0; - } else { - nv->value = mp_get_runtime_velocity(); - if (cm_get_units_mode(RUNTIME) == INCHES) nv->value *= INCHES_PER_MM; - } - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); -} - -stat_t cm_get_feed(nvObj_t *nv) -{ - nv->value = cm_get_feed_rate(ACTIVE_MODEL); - if (cm_get_units_mode(ACTIVE_MODEL) == INCHES) nv->value *= INCHES_PER_MM; - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); -} - -stat_t cm_get_pos(nvObj_t *nv) -{ - nv->value = cm_get_work_position(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); -} - -stat_t cm_get_mpo(nvObj_t *nv) -{ - nv->value = cm_get_absolute_position(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); -} - -stat_t cm_get_ofs(nvObj_t *nv) -{ - nv->value = cm_get_work_offset(ACTIVE_MODEL, _get_axis(nv->index)); - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); +stat_t cm_get_vel(nvObj_t *nv) { + if (cm_get_motion_state() == MOTION_STOP) nv->value = 0; + else { + nv->value = mp_get_runtime_velocity(); + if (cm_get_units_mode(RUNTIME) == INCHES) nv->value *= INCHES_PER_MM; + } + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return STAT_OK; } + +stat_t cm_get_feed(nvObj_t *nv) { + nv->value = cm_get_feed_rate(ACTIVE_MODEL); + if (cm_get_units_mode(ACTIVE_MODEL) == INCHES) nv->value *= INCHES_PER_MM; + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return STAT_OK; +} + + +stat_t cm_get_pos(nvObj_t *nv) { + nv->value = cm_get_work_position(ACTIVE_MODEL, _get_axis(nv->index)); + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return STAT_OK; +} + + +stat_t cm_get_mpo(nvObj_t *nv) { + nv->value = cm_get_absolute_position(ACTIVE_MODEL, _get_axis(nv->index)); + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return STAT_OK; +} + + +stat_t cm_get_ofs(nvObj_t *nv) { + nv->value = cm_get_work_offset(ACTIVE_MODEL, _get_axis(nv->index)); + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return STAT_OK; +} + + /* * AXIS GET AND SET FUNCTIONS * - * cm_get_am() - get axis mode w/enumeration string - * cm_set_am() - set axis mode w/exception handling for axis type - * cm_set_sw() - run this any time you change a switch setting + * cm_get_am() - get axis mode w/enumeration string + * cm_set_am() - set axis mode w/exception handling for axis type + * cm_set_sw() - run this any time you change a switch setting */ - -stat_t cm_get_am(nvObj_t *nv) -{ - get_ui8(nv); - return(_get_msg_helper(nv, msg_am, nv->value)); +stat_t cm_get_am(nvObj_t *nv) { + get_ui8(nv); + return(_get_msg_helper(nv, msg_am, nv->value)); } -stat_t cm_set_am(nvObj_t *nv) // axis mode -{ - if (_get_axis_type(nv->index) == 0) { // linear - if (nv->value > AXIS_MODE_MAX_LINEAR) { return (STAT_INPUT_EXCEEDS_MAX_VALUE);} - } else { - if (nv->value > AXIS_MODE_MAX_ROTARY) { return (STAT_INPUT_EXCEEDS_MAX_VALUE);} - } - set_ui8(nv); - return(STAT_OK); + +stat_t cm_set_am(nvObj_t *nv) { // axis mode + if (_get_axis_type(nv->index) == 0) { // linear + if (nv->value > AXIS_MODE_MAX_LINEAR) return STAT_INPUT_EXCEEDS_MAX_VALUE; + } else if (nv->value > AXIS_MODE_MAX_ROTARY) return STAT_INPUT_EXCEEDS_MAX_VALUE; + + set_ui8(nv); + return(STAT_OK); } /**** 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 * - * cm_set_xjm() - set jerk max value - * cm_set_xjh() - set jerk halt value (used by homing and other stops) + * 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 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 */ -float cm_get_axis_jerk(uint8_t axis) -{ - return (cm.a[axis].jerk_max); +float cm_get_axis_jerk(uint8_t axis) { + return cm.a[axis].jerk_max; } -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); + +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); } -stat_t cm_set_xjm(nvObj_t *nv) -{ - if (nv->value > JERK_MULTIPLIER) nv->value /= JERK_MULTIPLIER; - set_flu(nv); - cm_set_axis_jerk(_get_axis(nv->index), nv->value); - return(STAT_OK); + +stat_t cm_set_xjm(nvObj_t *nv) { + if (nv->value > JERK_MULTIPLIER) nv->value /= JERK_MULTIPLIER; + set_flu(nv); + cm_set_axis_jerk(_get_axis(nv->index), nv->value); + return(STAT_OK); } -stat_t cm_set_xjh(nvObj_t *nv) -{ - if (nv->value > JERK_MULTIPLIER) nv->value /= JERK_MULTIPLIER; - set_flu(nv); - return(STAT_OK); + +stat_t cm_set_xjh(nvObj_t *nv) { + if (nv->value > JERK_MULTIPLIER) nv->value /= JERK_MULTIPLIER; + set_flu(nv); + return(STAT_OK); } /* @@ -1700,84 +1628,78 @@ stat_t cm_set_xjh(nvObj_t *nv) * cm_run_qf() - flush planner queue * cm_run_home() - run homing sequence */ - -stat_t cm_run_qf(nvObj_t *nv) -{ - cm_request_queue_flush(); - return (STAT_OK); +stat_t cm_run_qf(nvObj_t *nv) { + cm_request_queue_flush(); + return STAT_OK; } -stat_t cm_run_home(nvObj_t *nv) -{ - if (fp_TRUE(nv->value)) { cm_homing_cycle_start();} - return (STAT_OK); + +stat_t cm_run_home(nvObj_t *nv) { + if (fp_TRUE(nv->value)) {cm_homing_cycle_start();} + return STAT_OK; } + /* * Debugging Commands * * cm_dam() - dump active model */ +stat_t cm_dam(nvObj_t *nv) { + printf("Active model:\n"); + cm_print_vel(nv); + cm_print_feed(nv); + cm_print_line(nv); + cm_print_stat(nv); + cm_print_macs(nv); + cm_print_cycs(nv); + cm_print_mots(nv); + cm_print_hold(nv); + cm_print_home(nv); + cm_print_unit(nv); + cm_print_coor(nv); + cm_print_momo(nv); + cm_print_plan(nv); + cm_print_path(nv); + cm_print_dist(nv); + cm_print_frmo(nv); + cm_print_tool(nv); -stat_t cm_dam(nvObj_t *nv) -{ - printf("Active model:\n"); - cm_print_vel(nv); - cm_print_feed(nv); - cm_print_line(nv); - cm_print_stat(nv); - cm_print_macs(nv); - cm_print_cycs(nv); - cm_print_mots(nv); - cm_print_hold(nv); - cm_print_home(nv); - cm_print_unit(nv); - cm_print_coor(nv); - cm_print_momo(nv); - cm_print_plan(nv); - cm_print_path(nv); - cm_print_dist(nv); - cm_print_frmo(nv); - cm_print_tool(nv); - - return (STAT_OK); + return STAT_OK; } -/*********************************************************************************** - * AXIS JOGGING - ***********************************************************************************/ -float cm_get_jogging_dest(void) -{ - return cm.jogging_dest; +// AXIS JOGGING +float cm_get_jogging_dest() { + return cm.jogging_dest; } -stat_t cm_run_jogx(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_X); - return (STAT_OK); + +stat_t cm_run_jogx(nvObj_t *nv) { + set_flt(nv); + cm_jogging_cycle_start(AXIS_X); + return STAT_OK; } -stat_t cm_run_jogy(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_Y); - return (STAT_OK); + +stat_t cm_run_jogy(nvObj_t *nv) { + set_flt(nv); + cm_jogging_cycle_start(AXIS_Y); + return STAT_OK; } -stat_t cm_run_jogz(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_Z); - return (STAT_OK); + +stat_t cm_run_jogz(nvObj_t *nv) { + set_flt(nv); + cm_jogging_cycle_start(AXIS_Z); + return STAT_OK; } -stat_t cm_run_joga(nvObj_t *nv) -{ - set_flt(nv); - cm_jogging_cycle_start(AXIS_A); - return (STAT_OK); + +stat_t cm_run_joga(nvObj_t *nv) { + set_flt(nv); + cm_jogging_cycle_start(AXIS_A); + return STAT_OK; } /*********************************************************************************** @@ -1818,32 +1740,31 @@ const char fmt_gco[] PROGMEM = "[gco] default gcode coord system%3d [1-6 (G54-G5 const char fmt_gpa[] PROGMEM = "[gpa] default gcode path control%3d [0=G61,1=G61.1,2=G64]\n"; const char fmt_gdi[] PROGMEM = "[gdi] default gcode distance mode%2d [0=G90,1=G91]\n"; -void cm_print_vel(nvObj_t *nv) { text_print_flt_units(nv, fmt_vel, GET_UNITS(ACTIVE_MODEL));} -void cm_print_feed(nvObj_t *nv) { text_print_flt_units(nv, fmt_feed, GET_UNITS(ACTIVE_MODEL));} -void cm_print_line(nvObj_t *nv) { text_print_int(nv, fmt_line);} -void cm_print_stat(nvObj_t *nv) { text_print_str(nv, fmt_stat);} -void cm_print_macs(nvObj_t *nv) { text_print_str(nv, fmt_macs);} -void cm_print_cycs(nvObj_t *nv) { text_print_str(nv, fmt_cycs);} -void cm_print_mots(nvObj_t *nv) { text_print_str(nv, fmt_mots);} -void cm_print_hold(nvObj_t *nv) { text_print_str(nv, fmt_hold);} -void cm_print_home(nvObj_t *nv) { text_print_str(nv, fmt_home);} -void cm_print_unit(nvObj_t *nv) { text_print_str(nv, fmt_unit);} -void cm_print_coor(nvObj_t *nv) { text_print_str(nv, fmt_coor);} -void cm_print_momo(nvObj_t *nv) { text_print_str(nv, fmt_momo);} -void cm_print_plan(nvObj_t *nv) { text_print_str(nv, fmt_plan);} -void cm_print_path(nvObj_t *nv) { text_print_str(nv, fmt_path);} -void cm_print_dist(nvObj_t *nv) { text_print_str(nv, fmt_dist);} -void cm_print_frmo(nvObj_t *nv) { text_print_str(nv, fmt_frmo);} -void cm_print_tool(nvObj_t *nv) { text_print_int(nv, fmt_tool);} - -void cm_print_gpl(nvObj_t *nv) { text_print_int(nv, fmt_gpl);} -void cm_print_gun(nvObj_t *nv) { text_print_int(nv, fmt_gun);} -void cm_print_gco(nvObj_t *nv) { text_print_int(nv, fmt_gco);} -void cm_print_gpa(nvObj_t *nv) { text_print_int(nv, fmt_gpa);} -void cm_print_gdi(nvObj_t *nv) { text_print_int(nv, fmt_gdi);} - -/* system state print functions */ - +void cm_print_vel(nvObj_t *nv) {text_print_flt_units(nv, fmt_vel, GET_UNITS(ACTIVE_MODEL));} +void cm_print_feed(nvObj_t *nv) {text_print_flt_units(nv, fmt_feed, GET_UNITS(ACTIVE_MODEL));} +void cm_print_line(nvObj_t *nv) {text_print_int(nv, fmt_line);} +void cm_print_stat(nvObj_t *nv) {text_print_str(nv, fmt_stat);} +void cm_print_macs(nvObj_t *nv) {text_print_str(nv, fmt_macs);} +void cm_print_cycs(nvObj_t *nv) {text_print_str(nv, fmt_cycs);} +void cm_print_mots(nvObj_t *nv) {text_print_str(nv, fmt_mots);} +void cm_print_hold(nvObj_t *nv) {text_print_str(nv, fmt_hold);} +void cm_print_home(nvObj_t *nv) {text_print_str(nv, fmt_home);} +void cm_print_unit(nvObj_t *nv) {text_print_str(nv, fmt_unit);} +void cm_print_coor(nvObj_t *nv) {text_print_str(nv, fmt_coor);} +void cm_print_momo(nvObj_t *nv) {text_print_str(nv, fmt_momo);} +void cm_print_plan(nvObj_t *nv) {text_print_str(nv, fmt_plan);} +void cm_print_path(nvObj_t *nv) {text_print_str(nv, fmt_path);} +void cm_print_dist(nvObj_t *nv) {text_print_str(nv, fmt_dist);} +void cm_print_frmo(nvObj_t *nv) {text_print_str(nv, fmt_frmo);} +void cm_print_tool(nvObj_t *nv) {text_print_int(nv, fmt_tool);} + +void cm_print_gpl(nvObj_t *nv) {text_print_int(nv, fmt_gpl);} +void cm_print_gun(nvObj_t *nv) {text_print_int(nv, fmt_gun);} +void cm_print_gco(nvObj_t *nv) {text_print_int(nv, fmt_gco);} +void cm_print_gpa(nvObj_t *nv) {text_print_int(nv, fmt_gpa);} +void cm_print_gdi(nvObj_t *nv) {text_print_int(nv, fmt_gdi);} + +// system state print functions const char fmt_ja[] PROGMEM = "[ja] junction acceleration%8.0f%s\n"; const char fmt_ct[] PROGMEM = "[ct] chordal tolerance%17.4f%s\n"; const char fmt_sl[] PROGMEM = "[sl] soft limit enable%12d\n"; @@ -1851,37 +1772,37 @@ const char fmt_ml[] PROGMEM = "[ml] min line segment%17.3f%s\n"; const char fmt_ma[] PROGMEM = "[ma] min arc segment%18.3f%s\n"; const char fmt_ms[] PROGMEM = "[ms] min segment time%13.0f uSec\n"; -void cm_print_ja(nvObj_t *nv) { text_print_flt_units(nv, fmt_ja, GET_UNITS(ACTIVE_MODEL));} -void cm_print_ct(nvObj_t *nv) { text_print_flt_units(nv, fmt_ct, GET_UNITS(ACTIVE_MODEL));} -void cm_print_sl(nvObj_t *nv) { text_print_ui8(nv, fmt_sl);} -void cm_print_ml(nvObj_t *nv) { text_print_flt_units(nv, fmt_ml, GET_UNITS(ACTIVE_MODEL));} -void cm_print_ma(nvObj_t *nv) { text_print_flt_units(nv, fmt_ma, GET_UNITS(ACTIVE_MODEL));} -void cm_print_ms(nvObj_t *nv) { text_print_flt_units(nv, fmt_ms, GET_UNITS(ACTIVE_MODEL));} +void cm_print_ja(nvObj_t *nv) {text_print_flt_units(nv, fmt_ja, GET_UNITS(ACTIVE_MODEL));} +void cm_print_ct(nvObj_t *nv) {text_print_flt_units(nv, fmt_ct, GET_UNITS(ACTIVE_MODEL));} +void cm_print_sl(nvObj_t *nv) {text_print_ui8(nv, fmt_sl);} +void cm_print_ml(nvObj_t *nv) {text_print_flt_units(nv, fmt_ml, GET_UNITS(ACTIVE_MODEL));} +void cm_print_ma(nvObj_t *nv) {text_print_flt_units(nv, fmt_ma, GET_UNITS(ACTIVE_MODEL));} +void cm_print_ms(nvObj_t *nv) {text_print_flt_units(nv, fmt_ms, GET_UNITS(ACTIVE_MODEL));} /* * axis print functions * - * _print_axis_ui8() - helper to print an integer value with no units - * _print_axis_flt() - helper to print a floating point linear value in prevailing units - * _print_pos_helper() - * - * cm_print_am() - * cm_print_fr() - * cm_print_vm() - * cm_print_tm() - * cm_print_tn() - * cm_print_jm() - * cm_print_jh() - * cm_print_jd() - * cm_print_ra() - * cm_print_sn() - * cm_print_sx() - * cm_print_lv() - * cm_print_lb() - * cm_print_zb() - * - * cm_print_pos() - print position with unit displays for MM or Inches - * cm_print_mpo() - print position with fixed unit display - always in Degrees or MM + * _print_axis_ui8() - helper to print an integer value with no units + * _print_axis_flt() - helper to print a floating point linear value in prevailing units + * _print_pos_helper() + * + * cm_print_am() + * cm_print_fr() + * cm_print_vm() + * cm_print_tm() + * cm_print_tn() + * cm_print_jm() + * cm_print_jh() + * cm_print_jd() + * cm_print_ra() + * cm_print_sn() + * cm_print_sx() + * cm_print_lv() + * cm_print_lb() + * cm_print_zb() + * + * cm_print_pos() - print position with unit displays for MM or Inches + * cm_print_mpo() - print position with fixed unit display - always in Degrees or MM */ static const char fmt_Xam[] PROGMEM = "[%s%s] %s axis mode%18d %s\n"; @@ -1902,67 +1823,66 @@ static const char fmt_Xzb[] PROGMEM = "[%s%s] %s zero backoff%19.3f%s\n"; static const char fmt_cofs[] PROGMEM = "[%s%s] %s %s offset%20.3f%s\n"; static const char fmt_cpos[] PROGMEM = "[%s%s] %s %s position%18.3f%s\n"; -static void _print_axis_ui8(nvObj_t *nv, const char *format) -{ - fprintf_P(stderr, format, nv->group, nv->token, nv->group, (uint8_t)nv->value); -} - -static void _print_axis_flt(nvObj_t *nv, const char *format) -{ - char *units; - if (_get_axis_type(nv->index) == 0) { // linear - units = (char *)GET_UNITS(MODEL); - } else { - units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); - } - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value, units); -} - -static void _print_axis_coord_flt(nvObj_t *nv, const char *format) -{ - char *units; - if (_get_axis_type(nv->index) == 0) { // linear - units = (char *)GET_UNITS(MODEL); - } else { - units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); - } - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->token, nv->value, units); -} - -static void _print_pos(nvObj_t *nv, const char *format, uint8_t units) -{ - char axes[] = {"XYZABC"}; - uint8_t axis = _get_axis(nv->index); - if (axis >= AXIS_A) { units = DEGREES;} - fprintf_P(stderr, format, axes[axis], nv->value, GET_TEXT_ITEM(msg_units, units)); -} - -void cm_print_am(nvObj_t *nv) // print axis mode with enumeration string -{ - fprintf_P(stderr, fmt_Xam, nv->group, nv->token, nv->group, (uint8_t)nv->value, - GET_TEXT_ITEM(msg_am, (uint8_t)nv->value)); -} - -void cm_print_fr(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xfr);} -void cm_print_vm(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xvm);} -void cm_print_tm(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xtm);} -void cm_print_tn(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xtn);} -void cm_print_jm(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xjm);} -void cm_print_jh(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xjh);} -void cm_print_jd(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xjd);} -void cm_print_ra(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xra);} -void cm_print_sn(nvObj_t *nv) { _print_axis_ui8(nv, fmt_Xsn);} -void cm_print_sx(nvObj_t *nv) { _print_axis_ui8(nv, fmt_Xsx);} -void cm_print_sv(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xsv);} -void cm_print_lv(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xlv);} -void cm_print_lb(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xlb);} -void cm_print_zb(nvObj_t *nv) { _print_axis_flt(nv, fmt_Xzb);} - -void cm_print_cofs(nvObj_t *nv) { _print_axis_coord_flt(nv, fmt_cofs);} -void cm_print_cpos(nvObj_t *nv) { _print_axis_coord_flt(nv, fmt_cpos);} - -void cm_print_pos(nvObj_t *nv) { _print_pos(nv, fmt_pos, cm_get_units_mode(MODEL));} -void cm_print_mpo(nvObj_t *nv) { _print_pos(nv, fmt_mpo, MILLIMETERS);} -void cm_print_ofs(nvObj_t *nv) { _print_pos(nv, fmt_ofs, MILLIMETERS);} + +static void _print_axis_ui8(nvObj_t *nv, const char *format) { + fprintf_P(stderr, format, nv->group, nv->token, nv->group, (uint8_t)nv->value); +} + + +static void _print_axis_flt(nvObj_t *nv, const char *format) { + char *units; + if (_get_axis_type(nv->index) == 0) // linear + units = (char *)GET_UNITS(MODEL); + else units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); + + fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value, units); +} + + +static void _print_axis_coord_flt(nvObj_t *nv, const char *format) { + char *units; + if (_get_axis_type(nv->index) == 0) // linear + units = (char *)GET_UNITS(MODEL); + else units = (char *)GET_TEXT_ITEM(msg_units, DEGREE_INDEX); + + fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->token, nv->value, units); +} + + +static void _print_pos(nvObj_t *nv, const char *format, uint8_t units) { + char axes[] = {"XYZABC"}; + uint8_t axis = _get_axis(nv->index); + if (axis >= AXIS_A) {units = DEGREES;} + fprintf_P(stderr, format, axes[axis], nv->value, GET_TEXT_ITEM(msg_units, units)); +} + + +void cm_print_am(nvObj_t *nv) { // print axis mode with enumeration string + fprintf_P(stderr, fmt_Xam, nv->group, nv->token, nv->group, (uint8_t)nv->value, + GET_TEXT_ITEM(msg_am, (uint8_t)nv->value)); +} + + +void cm_print_fr(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xfr);} +void cm_print_vm(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xvm);} +void cm_print_tm(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xtm);} +void cm_print_tn(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xtn);} +void cm_print_jm(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xjm);} +void cm_print_jh(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xjh);} +void cm_print_jd(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xjd);} +void cm_print_ra(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xra);} +void cm_print_sn(nvObj_t *nv) {_print_axis_ui8(nv, fmt_Xsn);} +void cm_print_sx(nvObj_t *nv) {_print_axis_ui8(nv, fmt_Xsx);} +void cm_print_sv(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xsv);} +void cm_print_lv(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xlv);} +void cm_print_lb(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xlb);} +void cm_print_zb(nvObj_t *nv) {_print_axis_flt(nv, fmt_Xzb);} + +void cm_print_cofs(nvObj_t *nv) {_print_axis_coord_flt(nv, fmt_cofs);} +void cm_print_cpos(nvObj_t *nv) {_print_axis_coord_flt(nv, fmt_cpos);} + +void cm_print_pos(nvObj_t *nv) {_print_pos(nv, fmt_pos, cm_get_units_mode(MODEL));} +void cm_print_mpo(nvObj_t *nv) {_print_pos(nv, fmt_mpo, MILLIMETERS);} +void cm_print_ofs(nvObj_t *nv) {_print_pos(nv, fmt_ofs, MILLIMETERS);} #endif // __TEXT_MODE diff --git a/src/canonical_machine.h b/src/canonical_machine.h index 4076952..529a6cb 100755 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -35,10 +35,10 @@ /* Defines, Macros, and Assorted Parameters */ -#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 +#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 #define _to_millimeters(a) ((cm.gm.units_mode == INCHES) ? (a * MM_PER_INCH) : a) @@ -49,137 +49,137 @@ * 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. + * 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. + * 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. + * 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(). + * 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). + * 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) +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) } GCodeState_t; -typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by model and arcs - uint16_t magic_start; // magic number to test memory integrity - uint8_t next_action; // handles G modal group 1 moves & non-modals - uint8_t program_flow; // used only by the gcode_parser +typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by model and arcs + uint16_t magic_start; // magic number to test memory integrity + uint8_t next_action; // handles G modal group 1 moves & non-modals + 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]; // 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 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 - 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 + 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 + 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. 0=disabled, 1=enabled + uint8_t block_delete_switch; // set true to enable block deletes (true is default) - float spindle_override_factor; // 1.0000 x S spindle speed. Go up or down from there - uint8_t spindle_override_enable; // TRUE = override enabled + float spindle_override_factor; // 1.0000 x S spindle speed. Go up or down from there + 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) - uint16_t magic_end; + uint16_t magic_end; } 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 - 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 - 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 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 tool; // Tool after T and M6 (tool_select and tool_change) - 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 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 - uint8_t spindle_override_enable; // TRUE = override enabled - - float parameter; // P - parameter used for dwell time in seconds, G10 coord select... - float arc_radius; // R - radius value in arc radius mode - float arc_offset[3]; // IJK - used by arc commands +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 + 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 + 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 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 tool; // Tool after T and M6 (tool_select and tool_change) + 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 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 + uint8_t spindle_override_enable; // TRUE = override enabled + + float parameter; // P - parameter used for dwell time in seconds, 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; @@ -188,175 +188,175 @@ typedef struct GCodeInput { // Gcode model inputs - meaning depends on contex */ 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; // 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 } cfgAxis_t; -typedef struct cmSingleton { // struct to manage cm globals and cycles - magic_t magic_start; // magic number to test memory integrity - - /**** Config variables (PUBLIC) ****/ - - // system group settings - float junction_acceleration; // centripetal acceleration max for cornering - 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 - - // gcode power-on default settings - defaults are not the same as the gm state - uint8_t coord_system; // G10 active coordinate system default - uint8_t select_plane; // G17,G18,G19 reset default - uint8_t units_mode; // G20,G21 reset default - uint8_t path_control; // G61,G61.1,G64 reset default - uint8_t distance_mode; // G90,G91 reset default - - // coordinate systems and offsets - float offset[COORDS+1][AXES]; // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 - - // settings for axes X,Y,Z,A B,C - cfgAxis_t a[AXES]; - - /**** Runtime variables (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 - uint8_t cycle_state; // cycs - uint8_t motion_state; // momo - uint8_t hold_state; // hold: feedhold sub-state machine - uint8_t homing_state; // home: homing cycle sub-state machine - uint8_t homed[AXES]; // individual axis homing flags - - uint8_t probe_state; // 1==success, 0==failed - float probe_results[AXES]; // probing results - - uint8_t g28_flag; // true = complete a G28 move - uint8_t g30_flag; // true = complete a G30 move - uint8_t deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them - 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) - float jogging_dest; // jogging direction as a relative move from current position - struct GCodeState *am; // active Gcode model is maintained by state management - - /**** Model states ****/ - GCodeState_t gm; // core gcode model state - GCodeStateX_t gmx; // extended gcode model state - GCodeInput_t gn; // gcode input values - transient - GCodeInput_t gf; // gcode input flags - transient - - magic_t magic_end; +typedef struct cmSingleton { // struct to manage cm globals and cycles + magic_t magic_start; // magic number to test memory integrity + + /**** Config variables (PUBLIC) ****/ + + // system group settings + float junction_acceleration; // centripetal acceleration max for cornering + 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 + + // gcode power-on default settings - defaults are not the same as the gm state + uint8_t coord_system; // G10 active coordinate system default + uint8_t select_plane; // G17,G18,G19 reset default + uint8_t units_mode; // G20,G21 reset default + uint8_t path_control; // G61,G61.1,G64 reset default + uint8_t distance_mode; // G90,G91 reset default + + // coordinate systems and offsets + float offset[COORDS+1][AXES]; // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 + + // settings for axes X,Y,Z,A B,C + cfgAxis_t a[AXES]; + + /**** Runtime variables (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 + uint8_t cycle_state; // cycs + uint8_t motion_state; // momo + uint8_t hold_state; // hold: feedhold sub-state machine + uint8_t homing_state; // home: homing cycle sub-state machine + uint8_t homed[AXES]; // individual axis homing flags + + uint8_t probe_state; // 1==success, 0==failed + float probe_results[AXES]; // probing results + + uint8_t g28_flag; // true = complete a G28 move + uint8_t g30_flag; // true = complete a G30 move + uint8_t deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them + 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) + float jogging_dest; // jogging direction as a relative move from current position + struct GCodeState *am; // active Gcode model is maintained by state management + + /**** Model states ****/ + GCodeState_t gm; // core gcode model state + GCodeStateX_t gmx; // extended gcode model state + GCodeInput_t gn; // gcode input values - transient + GCodeInput_t gf; // gcode input flags - transient + + magic_t magic_end; } cmSingleton_t; /**** Externs - See canonical_machine.c for allocation ****/ -extern cmSingleton_t cm; // canonical machine controller singleton +extern cmSingleton_t cm; // canonical machine controller singleton /***************************************************************************** * MACHINE STATE MODEL * * 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 + * - 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 +/* 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 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_JOG, // [10] jogging is treated as a cycle - COMBINED_SHUTDOWN, // [11] machine in hard alarm state (shutdown) +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_JOG, // [10] jogging is treated as a cycle + COMBINED_SHUTDOWN, // [11] 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 = 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) }; 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_JOG // jogging is treated as a specialized cycle + 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_JOG // jogging 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 = 0, // 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 = 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 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 = 0, // machine is not homed (0=false) + HOMING_HOMED = 1, // 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 = 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 }; /* The difference between NextAction and MotionMode is that NextAction is @@ -364,140 +364,140 @@ enum cmProbeState { // applies to cm.probe_state * 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) - 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_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 - NEXT_ACTION_GOTO_G30_POSITION, // G30 - NEXT_ACTION_SET_COORD_DATA, // G10 - NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 - NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 - NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 - NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 - NEXT_ACTION_DWELL, // G4 - NEXT_ACTION_STRAIGHT_PROBE // G38.2 +enum cmNextAction { // these are in order to optimized CASE statement + NEXT_ACTION_DEFAULT = 0, // 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_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 + NEXT_ACTION_GOTO_G30_POSITION, // G30 + NEXT_ACTION_SET_COORD_DATA, // G10 + NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 + NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 + NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 + NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 + NEXT_ACTION_DWELL, // G4 + NEXT_ACTION_STRAIGHT_PROBE // G38.2 }; -enum cmMotionMode { // G Modal Group 1 - MOTION_MODE_STRAIGHT_TRAVERSE=0, // 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 - MOTION_MODE_CANCEL_MOTION_MODE, // G80 - MOTION_MODE_STRAIGHT_PROBE, // G38.2 - MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling - MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell - MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling - MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping - MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out - MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out - MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring - MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out - MOTION_MODE_CANNED_CYCLE_89 // G89 - boring, dwell, feed out +enum cmMotionMode { // G Modal Group 1 + MOTION_MODE_STRAIGHT_TRAVERSE=0, // 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 + MOTION_MODE_CANCEL_MOTION_MODE, // G80 + MOTION_MODE_STRAIGHT_PROBE, // G38.2 + MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling + MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell + MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling + MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping + MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out + MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out + MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring + MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out + MOTION_MODE_CANNED_CYCLE_89 // G89 - boring, dwell, feed out }; -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 = 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 }; #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 -enum cmCanonicalPlane { // canonical plane - translates to: - // axis_0 axis_1 axis_2 - CANON_PLANE_XY = 0, // G17 X Y Z - CANON_PLANE_XZ, // G18 X Z Y - CANON_PLANE_YZ // G19 Y Z X +enum cmCanonicalPlane { // canonical plane - translates to: + // axis_0 axis_1 axis_2 + CANON_PLANE_XY = 0, // 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 = 0, // G20 + MILLIMETERS, // G21 + DEGREES // ABC axes (this value used for displays only) }; enum cmCoordSystem { - ABSOLUTE_COORDS = 0, // machine coordinate system - G54, // G54 coordinate system - G55, // G55 coordinate system - G56, // G56 coordinate system - G57, // G57 coordinate system - G58, // G58 coordinate system - G59 // G59 coordinate system + ABSOLUTE_COORDS = 0, // machine coordinate system + G54, // G54 coordinate system + G55, // G55 coordinate system + G56, // G56 coordinate system + G57, // G57 coordinate system + G58, // G58 coordinate system + G59 // G59 coordinate system }; -#define COORD_SYSTEM_MAX G59 // set this manually to the last one +#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. - PATH_EXACT_STOP, // G61.1 - stops at all corners - PATH_CONTINUOUS // G64 and typically the default mode +enum cmPathControlMode { // G Modal Group 13 + PATH_EXACT_PATH = 0, // G61 - hits corners but does not stop if it does not need to. + PATH_EXACT_STOP, // G61.1 - stops at all corners + PATH_CONTINUOUS // G64 and typically the default mode }; enum cmDistanceMode { - ABSOLUTE_MODE = 0, // G90 - INCREMENTAL_MODE // G91 + ABSOLUTE_MODE = 0, // G90 + INCREMENTAL_MODE // G91 }; enum cmFeedRateMode { - 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) }; 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=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 }; enum cmProgramFlow { - PROGRAM_STOP = 0, - PROGRAM_END + PROGRAM_STOP = 0, + PROGRAM_END }; -enum cmSpindleState { // spindle state settings (See hardware.h for bit settings) - SPINDLE_OFF = 0, - SPINDLE_CW, - SPINDLE_CCW +enum cmSpindleState { // spindle state settings (See hardware.h for bit settings) + SPINDLE_OFF = 0, + 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 +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 }; -enum cmDirection { // used for spindle and arc dir - DIRECTION_CW = 0, - DIRECTION_CCW +enum cmDirection { // used for spindle and arc dir + DIRECTION_CW = 0, + 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 -}; // ordering must be preserved. See cm_set_move_times() +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 +}; // ordering must be preserved. See cm_set_move_times() #define AXIS_MODE_MAX_LINEAR AXIS_INHIBITED #define AXIS_MODE_MAX_ROTARY AXIS_RADIUS @@ -509,13 +509,13 @@ enum cmAxisMode { // axis modes (ordered: see _cm_get_feed_time()) /*--- Internal functions and helpers ---*/ // Model state getters and setters -uint8_t cm_get_combined_state(void); -uint8_t cm_get_machine_state(void); -uint8_t cm_get_cycle_state(void); -uint8_t cm_get_motion_state(void); -uint8_t cm_get_hold_state(void); -uint8_t cm_get_homing_state(void); -uint8_t cm_get_jogging_state(void); +uint8_t cm_get_combined_state(); +uint8_t cm_get_machine_state(); +uint8_t cm_get_cycle_state(); +uint8_t cm_get_motion_state(); +uint8_t cm_get_hold_state(); +uint8_t cm_get_homing_state(); +uint8_t cm_get_jogging_state(); void cm_set_motion_state(uint8_t motion_state); float cm_get_axis_jerk(uint8_t axis); void cm_set_axis_jerk(uint8_t axis, float jerk); @@ -530,8 +530,8 @@ uint8_t cm_get_distance_mode(GCodeState_t *gcode_state); uint8_t cm_get_feed_rate_mode(GCodeState_t *gcode_state); uint8_t cm_get_tool(GCodeState_t *gcode_state); uint8_t cm_get_spindle_mode(GCodeState_t *gcode_state); -uint8_t cm_get_block_delete_switch(void); -uint8_t cm_get_runtime_busy(void); +uint8_t cm_get_block_delete_switch(); +uint8_t cm_get_runtime_busy(); float cm_get_feed_rate(GCodeState_t *gcode_state); void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode); @@ -549,268 +549,268 @@ float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t axis); float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis); // Critical helpers -void cm_update_model_position_from_runtime(void); -void cm_finalize_move(void); -stat_t cm_deferred_write_callback(void); +void cm_update_model_position_from_runtime(); +void cm_finalize_move(); +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] ---*/ // Initialization and termination (4.3.2) -void canonical_machine_init(void); -void canonical_machine_init_assertions(void); -stat_t canonical_machine_test_assertions(void); +void canonical_machine_init(); +void canonical_machine_init_assertions(); +stat_t canonical_machine_test_assertions(); -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 +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 stat_t cm_clear(nvObj_t *nv); // 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_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 -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); // 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 -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(void); // G92.1 -stat_t cm_suspend_origin_offsets(void); // G92.2 -stat_t cm_resume_origin_offsets(void); // G92.3 +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 // Free Space Motion (4.3.4) -stat_t cm_straight_traverse(float target[], float flags[]); // G0 -stat_t cm_set_g28_position(void); // G28.1 -stat_t cm_goto_g28_position(float target[], float flags[]); // G28 -stat_t cm_set_g30_position(void); // G30.1 -stat_t cm_goto_g30_position(float target[], float flags[]); // G30 +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 // 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); // 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 // 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 - float i, float j, float k, - float radius, uint8_t motion_mode); -stat_t cm_dwell(float seconds); // G4, P parameter +stat_t cm_straight_feed(float target[], float flags[]); // G1 +stat_t cm_arc_feed( float target[], float flags[], // G2, G3 + float i, float j, float k, + float radius, uint8_t motion_mode); +stat_t cm_dwell(float seconds); // G4, P parameter // 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); // T parameter +stat_t cm_change_tool(uint8_t tool); // M6 // 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); // M7 +stat_t cm_flood_coolant_control(uint8_t flood_coolant); // M8, M9 -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); // 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 -void cm_message(char_t *message); // msg to console (e.g. Gcode comments) +void cm_message(char_t *message); // msg to console (e.g. Gcode comments) // Program Functions (4.3.10) -void cm_request_feedhold(void); -void cm_request_queue_flush(void); -void cm_request_cycle_start(void); +void cm_request_feedhold(); +void cm_request_queue_flush(); +void cm_request_cycle_start(); -stat_t cm_feedhold_sequencing_callback(void); // process feedhold, cycle start and queue flush requests -stat_t cm_queue_flush(void); // flush serial and planner queues with coordinate resets +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 -void cm_cycle_start(void); // (no Gcode) -void cm_cycle_end(void); // (no Gcode) -void cm_feedhold(void); // (no Gcode) -void cm_program_stop(void); // M0 -void cm_optional_program_stop(void); // M1 -void cm_program_end(void); // M2 +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 /*--- Cycles ---*/ // Homing cycles -stat_t cm_homing_cycle_start(void); // G28.2 -stat_t cm_homing_cycle_start_no_set(void); // G28.4 -stat_t cm_homing_callback(void); // G28.2/.4 main loop callback +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 // Probe cycles -stat_t cm_straight_probe(float target[], float flags[]); // G38.2 -stat_t cm_probe_callback(void); // G38.2 main loop callback +stat_t cm_straight_probe(float target[], float flags[]); // G38.2 +stat_t cm_probe_callback(); // G38.2 main loop callback // Jogging cycle -stat_t cm_jogging_callback(void); // jogging cycle main loop -stat_t cm_jogging_cycle_start(uint8_t axis); // {"jogx":-100.3} -float cm_get_jogging_dest(void); +stat_t cm_jogging_callback(); // jogging cycle main loop +stat_t cm_jogging_cycle_start(uint8_t axis); // {"jogx":-100.3} +float cm_get_jogging_dest(); /*--- cfgArray interface functions ---*/ char_t cm_get_axis_char(const int8_t axis); -stat_t cm_get_mline(nvObj_t *nv); // get model line number -stat_t cm_get_line(nvObj_t *nv); // get active (model or runtime) line number -stat_t cm_get_stat(nvObj_t *nv); // get combined machine state as value and string -stat_t cm_get_macs(nvObj_t *nv); // get raw machine state as value and string -stat_t cm_get_cycs(nvObj_t *nv); // get raw cycle state (etc etc)... -stat_t cm_get_mots(nvObj_t *nv); // get raw motion state... -stat_t cm_get_hold(nvObj_t *nv); // get raw hold state... -stat_t cm_get_home(nvObj_t *nv); // get raw homing state... -stat_t cm_get_unit(nvObj_t *nv); // get unit mode... -stat_t cm_get_coor(nvObj_t *nv); // get coordinate system in effect... -stat_t cm_get_momo(nvObj_t *nv); // get motion mode... -stat_t cm_get_plan(nvObj_t *nv); // get active plane... -stat_t cm_get_path(nvObj_t *nv); // get patch control mode... -stat_t cm_get_dist(nvObj_t *nv); // get distance mode... -stat_t cm_get_frmo(nvObj_t *nv); // get feedrate mode... -stat_t cm_get_toolv(nvObj_t *nv); // get tool (value) -stat_t cm_get_pwr(nvObj_t *nv); // get motor power enable state - -stat_t cm_get_vel(nvObj_t *nv); // get runtime velocity... +stat_t cm_get_mline(nvObj_t *nv); // get model line number +stat_t cm_get_line(nvObj_t *nv); // get active (model or runtime) line number +stat_t cm_get_stat(nvObj_t *nv); // get combined machine state as value and string +stat_t cm_get_macs(nvObj_t *nv); // get raw machine state as value and string +stat_t cm_get_cycs(nvObj_t *nv); // get raw cycle state (etc etc)... +stat_t cm_get_mots(nvObj_t *nv); // get raw motion state... +stat_t cm_get_hold(nvObj_t *nv); // get raw hold state... +stat_t cm_get_home(nvObj_t *nv); // get raw homing state... +stat_t cm_get_unit(nvObj_t *nv); // get unit mode... +stat_t cm_get_coor(nvObj_t *nv); // get coordinate system in effect... +stat_t cm_get_momo(nvObj_t *nv); // get motion mode... +stat_t cm_get_plan(nvObj_t *nv); // get active plane... +stat_t cm_get_path(nvObj_t *nv); // get patch control mode... +stat_t cm_get_dist(nvObj_t *nv); // get distance mode... +stat_t cm_get_frmo(nvObj_t *nv); // get feedrate mode... +stat_t cm_get_toolv(nvObj_t *nv); // get tool (value) +stat_t cm_get_pwr(nvObj_t *nv); // get motor power enable state + +stat_t cm_get_vel(nvObj_t *nv); // get runtime velocity... stat_t cm_get_feed(nvObj_t *nv); -stat_t cm_get_pos(nvObj_t *nv); // get runtime work position... -stat_t cm_get_mpo(nvObj_t *nv); // get runtime machine position... -stat_t cm_get_ofs(nvObj_t *nv); // get runtime work offset... +stat_t cm_get_pos(nvObj_t *nv); // get runtime work position... +stat_t cm_get_mpo(nvObj_t *nv); // get runtime machine position... +stat_t cm_get_ofs(nvObj_t *nv); // get runtime work offset... -stat_t cm_run_qf(nvObj_t *nv); // run queue flush -stat_t cm_run_home(nvObj_t *nv); // start homing cycle +stat_t cm_run_qf(nvObj_t *nv); // run queue flush +stat_t cm_run_home(nvObj_t *nv); // start homing cycle -stat_t cm_dam(nvObj_t *nv); // dump active model (debugging command) +stat_t cm_dam(nvObj_t *nv); // dump active model (debugging command) -stat_t cm_run_jogx(nvObj_t *nv); // start jogging cycle for x -stat_t cm_run_jogy(nvObj_t *nv); // start jogging cycle for y -stat_t cm_run_jogz(nvObj_t *nv); // start jogging cycle for z -stat_t cm_run_joga(nvObj_t *nv); // start jogging cycle for a +stat_t cm_run_jogx(nvObj_t *nv); // start jogging cycle for x +stat_t cm_run_jogy(nvObj_t *nv); // start jogging cycle for y +stat_t cm_run_jogz(nvObj_t *nv); // start jogging cycle for z +stat_t cm_run_joga(nvObj_t *nv); // start jogging cycle for a -stat_t cm_get_am(nvObj_t *nv); // get axis mode -stat_t cm_set_am(nvObj_t *nv); // set axis mode -stat_t cm_set_xjm(nvObj_t *nv); // set jerk max with 1,000,000 correction -stat_t cm_set_xjh(nvObj_t *nv); // set jerk homing with 1,000,000 correction +stat_t cm_get_am(nvObj_t *nv); // get axis mode +stat_t cm_set_am(nvObj_t *nv); // set axis mode +stat_t cm_set_xjm(nvObj_t *nv); // set jerk max with 1,000,000 correction +stat_t cm_set_xjh(nvObj_t *nv); // set jerk homing with 1,000,000 correction /*--- text_mode support functions ---*/ #ifdef __TEXT_MODE - void cm_print_vel(nvObj_t *nv); // model state reporting - void cm_print_feed(nvObj_t *nv); - void cm_print_line(nvObj_t *nv); - void cm_print_stat(nvObj_t *nv); - void cm_print_macs(nvObj_t *nv); - void cm_print_cycs(nvObj_t *nv); - void cm_print_mots(nvObj_t *nv); - void cm_print_hold(nvObj_t *nv); - void cm_print_home(nvObj_t *nv); - void cm_print_unit(nvObj_t *nv); - void cm_print_coor(nvObj_t *nv); - void cm_print_momo(nvObj_t *nv); - void cm_print_plan(nvObj_t *nv); - void cm_print_path(nvObj_t *nv); - void cm_print_dist(nvObj_t *nv); - void cm_print_frmo(nvObj_t *nv); - void cm_print_tool(nvObj_t *nv); - - void cm_print_gpl(nvObj_t *nv); // Gcode defaults - void cm_print_gun(nvObj_t *nv); - void cm_print_gco(nvObj_t *nv); - void cm_print_gpa(nvObj_t *nv); - void cm_print_gdi(nvObj_t *nv); - - void cm_print_lin(nvObj_t *nv); // generic print for linear values - void cm_print_pos(nvObj_t *nv); // print runtime work position in prevailing units - void cm_print_mpo(nvObj_t *nv); // print runtime work position always in MM units - void cm_print_ofs(nvObj_t *nv); // print runtime work offset always in MM units - - void cm_print_ja(nvObj_t *nv); // global CM settings - void cm_print_ct(nvObj_t *nv); - void cm_print_sl(nvObj_t *nv); - void cm_print_ml(nvObj_t *nv); - void cm_print_ma(nvObj_t *nv); - void cm_print_ms(nvObj_t *nv); - void cm_print_st(nvObj_t *nv); - - void cm_print_am(nvObj_t *nv); // axis print functions - void cm_print_fr(nvObj_t *nv); - void cm_print_vm(nvObj_t *nv); - void cm_print_tm(nvObj_t *nv); - void cm_print_tn(nvObj_t *nv); - void cm_print_jm(nvObj_t *nv); - void cm_print_jh(nvObj_t *nv); - void cm_print_jd(nvObj_t *nv); - void cm_print_ra(nvObj_t *nv); - void cm_print_sn(nvObj_t *nv); - void cm_print_sx(nvObj_t *nv); - void cm_print_sv(nvObj_t *nv); - void cm_print_lv(nvObj_t *nv); - void cm_print_lb(nvObj_t *nv); - void cm_print_zb(nvObj_t *nv); - void cm_print_cofs(nvObj_t *nv); - void cm_print_cpos(nvObj_t *nv); + void cm_print_vel(nvObj_t *nv); // model state reporting + void cm_print_feed(nvObj_t *nv); + void cm_print_line(nvObj_t *nv); + void cm_print_stat(nvObj_t *nv); + void cm_print_macs(nvObj_t *nv); + void cm_print_cycs(nvObj_t *nv); + void cm_print_mots(nvObj_t *nv); + void cm_print_hold(nvObj_t *nv); + void cm_print_home(nvObj_t *nv); + void cm_print_unit(nvObj_t *nv); + void cm_print_coor(nvObj_t *nv); + void cm_print_momo(nvObj_t *nv); + void cm_print_plan(nvObj_t *nv); + void cm_print_path(nvObj_t *nv); + void cm_print_dist(nvObj_t *nv); + void cm_print_frmo(nvObj_t *nv); + void cm_print_tool(nvObj_t *nv); + + void cm_print_gpl(nvObj_t *nv); // Gcode defaults + void cm_print_gun(nvObj_t *nv); + void cm_print_gco(nvObj_t *nv); + void cm_print_gpa(nvObj_t *nv); + void cm_print_gdi(nvObj_t *nv); + + void cm_print_lin(nvObj_t *nv); // generic print for linear values + void cm_print_pos(nvObj_t *nv); // print runtime work position in prevailing units + void cm_print_mpo(nvObj_t *nv); // print runtime work position always in MM units + void cm_print_ofs(nvObj_t *nv); // print runtime work offset always in MM units + + void cm_print_ja(nvObj_t *nv); // global CM settings + void cm_print_ct(nvObj_t *nv); + void cm_print_sl(nvObj_t *nv); + void cm_print_ml(nvObj_t *nv); + void cm_print_ma(nvObj_t *nv); + void cm_print_ms(nvObj_t *nv); + void cm_print_st(nvObj_t *nv); + + void cm_print_am(nvObj_t *nv); // axis print functions + void cm_print_fr(nvObj_t *nv); + void cm_print_vm(nvObj_t *nv); + void cm_print_tm(nvObj_t *nv); + void cm_print_tn(nvObj_t *nv); + void cm_print_jm(nvObj_t *nv); + void cm_print_jh(nvObj_t *nv); + void cm_print_jd(nvObj_t *nv); + void cm_print_ra(nvObj_t *nv); + void cm_print_sn(nvObj_t *nv); + void cm_print_sx(nvObj_t *nv); + void cm_print_sv(nvObj_t *nv); + void cm_print_lv(nvObj_t *nv); + void cm_print_lb(nvObj_t *nv); + void cm_print_zb(nvObj_t *nv); + void cm_print_cofs(nvObj_t *nv); + void cm_print_cpos(nvObj_t *nv); #else // __TEXT_MODE - #define cm_print_vel tx_print_stub // model state reporting - #define cm_print_feed tx_print_stub - #define cm_print_line tx_print_stub - #define cm_print_stat tx_print_stub - #define cm_print_macs tx_print_stub - #define cm_print_cycs tx_print_stub - #define cm_print_mots tx_print_stub - #define cm_print_hold tx_print_stub - #define cm_print_home tx_print_stub - #define cm_print_unit tx_print_stub - #define cm_print_coor tx_print_stub - #define cm_print_momo tx_print_stub - #define cm_print_plan tx_print_stub - #define cm_print_path tx_print_stub - #define cm_print_dist tx_print_stub - #define cm_print_frmo tx_print_stub - #define cm_print_tool tx_print_stub - - #define cm_print_gpl tx_print_stub // Gcode defaults - #define cm_print_gun tx_print_stub - #define cm_print_gco tx_print_stub - #define cm_print_gpa tx_print_stub - #define cm_print_gdi tx_print_stub - - #define cm_print_lin tx_print_stub // generic print for linear values - #define cm_print_pos tx_print_stub // print runtime work position in prevailing units - #define cm_print_mpo tx_print_stub // print runtime work position always in MM uints - #define cm_print_ofs tx_print_stub // print runtime work offset always in MM uints - - #define cm_print_ja tx_print_stub // global CM settings - #define cm_print_ct tx_print_stub - #define cm_print_sl tx_print_stub - #define cm_print_ml tx_print_stub - #define cm_print_ma tx_print_stub - #define cm_print_ms tx_print_stub - #define cm_print_st tx_print_stub - - #define cm_print_am tx_print_stub // axis print functions - #define cm_print_fr tx_print_stub - #define cm_print_vm tx_print_stub - #define cm_print_tm tx_print_stub - #define cm_print_tn tx_print_stub - #define cm_print_jm tx_print_stub - #define cm_print_jh tx_print_stub - #define cm_print_jd tx_print_stub - #define cm_print_ra tx_print_stub - #define cm_print_sn tx_print_stub - #define cm_print_sx tx_print_stub - #define cm_print_sv tx_print_stub - #define cm_print_lv tx_print_stub - #define cm_print_lb tx_print_stub - #define cm_print_zb tx_print_stub - #define cm_print_cofs tx_print_stub - #define cm_print_cpos tx_print_stub + #define cm_print_vel tx_print_stub // model state reporting + #define cm_print_feed tx_print_stub + #define cm_print_line tx_print_stub + #define cm_print_stat tx_print_stub + #define cm_print_macs tx_print_stub + #define cm_print_cycs tx_print_stub + #define cm_print_mots tx_print_stub + #define cm_print_hold tx_print_stub + #define cm_print_home tx_print_stub + #define cm_print_unit tx_print_stub + #define cm_print_coor tx_print_stub + #define cm_print_momo tx_print_stub + #define cm_print_plan tx_print_stub + #define cm_print_path tx_print_stub + #define cm_print_dist tx_print_stub + #define cm_print_frmo tx_print_stub + #define cm_print_tool tx_print_stub + + #define cm_print_gpl tx_print_stub // Gcode defaults + #define cm_print_gun tx_print_stub + #define cm_print_gco tx_print_stub + #define cm_print_gpa tx_print_stub + #define cm_print_gdi tx_print_stub + + #define cm_print_lin tx_print_stub // generic print for linear values + #define cm_print_pos tx_print_stub // print runtime work position in prevailing units + #define cm_print_mpo tx_print_stub // print runtime work position always in MM uints + #define cm_print_ofs tx_print_stub // print runtime work offset always in MM uints + + #define cm_print_ja tx_print_stub // global CM settings + #define cm_print_ct tx_print_stub + #define cm_print_sl tx_print_stub + #define cm_print_ml tx_print_stub + #define cm_print_ma tx_print_stub + #define cm_print_ms tx_print_stub + #define cm_print_st tx_print_stub + + #define cm_print_am tx_print_stub // axis print functions + #define cm_print_fr tx_print_stub + #define cm_print_vm tx_print_stub + #define cm_print_tm tx_print_stub + #define cm_print_tn tx_print_stub + #define cm_print_jm tx_print_stub + #define cm_print_jh tx_print_stub + #define cm_print_jd tx_print_stub + #define cm_print_ra tx_print_stub + #define cm_print_sn tx_print_stub + #define cm_print_sx tx_print_stub + #define cm_print_sv tx_print_stub + #define cm_print_lv tx_print_stub + #define cm_print_lb tx_print_stub + #define cm_print_zb tx_print_stub + #define cm_print_cofs tx_print_stub + #define cm_print_cpos tx_print_stub #endif // __TEXT_MODE #endif // End of include guard: CANONICAL_MACHINE_H_ONCE diff --git a/src/config.c b/src/config.c index 93d92c4..f6ab617 100755 --- a/src/config.c +++ b/src/config.c @@ -25,11 +25,11 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* - * See config.h for a Config system overview and a bunch of details. + * See config.h for a Config system overview and a bunch of details. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "report.h" #include "controller.h" #include "canonical_machine.h" @@ -39,7 +39,7 @@ #include "hardware.h" #include "help.h" #include "util.h" -#include "xio.h" +#include "xio/xio.h" static void _set_defa(nvObj_t *nv); @@ -56,80 +56,78 @@ nvList_t nvl; /* Primary access points to functions bound to text mode / JSON functions * These gatekeeper functions check index ranges so others don't have to * - * nv_set() - Write a value or invoke a function - operates on single valued elements or groups - * nv_get() - Build a nvObj with the values from the target & return the value - * Populate nv body with single valued elements or groups (iterates) - * nv_print() - Output a formatted string for the value. + * nv_set() - Write a value or invoke a function - operates on single valued elements or groups + * nv_get() - Build a nvObj with the values from the target & return the value + * Populate nv body with single valued elements or groups (iterates) + * nv_print() - Output a formatted string for the value. * nv_persist() - persist value to non-volatile storage. Takes special cases into account * - * !!!! NOTE: nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround + * !!!! NOTE: nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround */ stat_t nv_set(nvObj_t *nv) { - if (nv->index >= nv_index_max()) + if (nv->index >= nv_index_max()) return(STAT_INTERNAL_RANGE_ERROR); - return (((fptrCmd)GET_TABLE_WORD(set))(nv)); + return ((fptrCmd)GET_TABLE_WORD(set))(nv); } stat_t nv_get(nvObj_t *nv) { - if (nv->index >= nv_index_max()) - return(STAT_INTERNAL_RANGE_ERROR); - return (((fptrCmd)GET_TABLE_WORD(get))(nv)); + if (nv->index >= nv_index_max()) return STAT_INTERNAL_RANGE_ERROR; + return ((fptrCmd)GET_TABLE_WORD(get))(nv); } void nv_print(nvObj_t *nv) { - if (nv->index >= nv_index_max()) return; - ((fptrCmd)GET_TABLE_WORD(print))(nv); + if (nv->index >= nv_index_max()) return; + ((fptrCmd)GET_TABLE_WORD(print))(nv); } -stat_t nv_persist(nvObj_t *nv) // nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround +stat_t nv_persist(nvObj_t *nv) // nv_persist() cannot be called from an interrupt on the AVR due to the AVR1008 EEPROM workaround { -#ifndef __DISABLE_PERSISTENCE // cutout for faster simulation in test - if (nv_index_lt_groups(nv->index) == false) +#ifndef __DISABLE_PERSISTENCE // cutout for faster simulation in test + if (nv_index_lt_groups(nv->index) == false) return(STAT_INTERNAL_RANGE_ERROR); - if (GET_TABLE_BYTE(flags) & F_PERSIST) + if (GET_TABLE_BYTE(flags) & F_PERSIST) return(write_persistent_value(nv)); #endif - return (STAT_OK); + return STAT_OK; } /************************************************************************************ * config_init() - called once on hard reset * * Performs one of 2 actions: - * (1) if persistence is set up or out-of-rev load RAM and NVM with settings.h defaults - * (2) if persistence is set up and at current config version use NVM data for config + * (1) if persistence is set up or out-of-rev load RAM and NVM with settings.h defaults + * (2) if persistence is set up and at current config version use NVM data for config * - * You can assume the cfg struct has been zeroed by a hard reset. - * Do not clear it as the version and build numbers have already been set by tg_init() + * You can assume the cfg struct has been zeroed by a hard reset. + * Do not clear it as the version and build numbers have already been set by tg_init() * * NOTE: Config assertions are handled from the controller */ void config_init() { - nvObj_t *nv = nv_reset_nv_list(); - config_init_assertions(); - - cm_set_units_mode(MILLIMETERS); // must do inits in millimeter mode - nv->index = 0; // this will read the first record in NVM - - read_persistent_value(nv); - if (nv->value != cs.fw_build) { // case (1) NVM is not setup or not in revision -// if (fp_NE(nv->value, cs.fw_build)) { - _set_defa(nv); - } else { // case (2) NVM is setup and in revision - rpt_print_loading_configs_message(); - for (nv->index=0; nv_index_is_single(nv->index); nv->index++) { - if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { - strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); // read the token from the array - read_persistent_value(nv); - nv_set(nv); - } - } - sr_init_status_report(); - } + nvObj_t *nv = nv_reset_nv_list(); + config_init_assertions(); + + cm_set_units_mode(MILLIMETERS); // must do inits in millimeter mode + nv->index = 0; // this will read the first record in NVM + + read_persistent_value(nv); + if (nv->value != cs.fw_build) { // case (1) NVM is not setup or not in revision + _set_defa(nv); + } else { // case (2) NVM is setup and in revision + rpt_print_loading_configs_message(); + for (nv->index=0; nv_index_is_single(nv->index); nv->index++) { + if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { + strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); // read the token from the array + read_persistent_value(nv); + nv_set(nv); + } + } + sr_init_status_report(); + } } /* @@ -139,31 +137,29 @@ void config_init() static void _set_defa(nvObj_t *nv) { - cm_set_units_mode(MILLIMETERS); // must do inits in MM mode - for (nv->index=0; nv_index_is_single(nv->index); nv->index++) { - if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { - nv->value = GET_TABLE_FLOAT(def_value); - strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); - nv_set(nv); - nv_persist(nv); - } - } - sr_init_status_report(); // reset status reports - rpt_print_initializing_message(); // don't start TX until all the NVM persistence is done + cm_set_units_mode(MILLIMETERS); // must do inits in MM mode + for (nv->index=0; nv_index_is_single(nv->index); nv->index++) { + if (GET_TABLE_BYTE(flags) & F_INITIALIZE) { + nv->value = GET_TABLE_FLOAT(def_value); + strncpy_P(nv->token, cfgArray[nv->index].token, TOKEN_LEN); + nv_set(nv); + nv_persist(nv); + } + } + sr_init_status_report(); // reset status reports + rpt_print_initializing_message(); // don't start TX until all the NVM persistence is done } stat_t set_defaults(nvObj_t *nv) { - // failsafe. nv->value must be true or no action occurs - if (fp_FALSE(nv->value)) return(help_defa(nv)); - _set_defa(nv); + // failsafe. nv->value must be true or no action occurs + if (fp_FALSE(nv->value)) return(help_defa(nv)); + _set_defa(nv); - // The values in nv are now garbage. Mark the nv as $defa so it displays nicely. -// strncpy(nv->token, "defa", TOKEN_LEN); // correct, but not required -// nv->index = nv_get_index("", nv->token); // correct, but not required - nv->valuetype = TYPE_INTEGER; - nv->value = 1; - return (STAT_OK); + // The values in nv are now garbage. Mark the nv as $defa so it displays nicely. + nv->valuetype = TYPE_INTEGER; + nv->value = 1; + return STAT_OK; } /* @@ -173,226 +169,225 @@ stat_t set_defaults(nvObj_t *nv) void config_init_assertions() { - cfg.magic_start = MAGICNUM; - cfg.magic_end = MAGICNUM; - nvl.magic_start = MAGICNUM; - nvl.magic_end = MAGICNUM; - nvStr.magic_start = MAGICNUM; - nvStr.magic_end = MAGICNUM; + cfg.magic_start = MAGICNUM; + cfg.magic_end = MAGICNUM; + nvl.magic_start = MAGICNUM; + nvl.magic_end = MAGICNUM; + nvStr.magic_start = MAGICNUM; + nvStr.magic_end = MAGICNUM; } stat_t config_test_assertions() { - if ((cfg.magic_start != MAGICNUM) || (cfg.magic_end != MAGICNUM)) return (STAT_CONFIG_ASSERTION_FAILURE); - if ((nvl.magic_start != MAGICNUM) || (nvl.magic_end != MAGICNUM)) return (STAT_CONFIG_ASSERTION_FAILURE); - if ((nvStr.magic_start != MAGICNUM) || (nvStr.magic_end != MAGICNUM)) return (STAT_CONFIG_ASSERTION_FAILURE); - if (global_string_buf[MESSAGE_LEN-1] != NUL) return (STAT_CONFIG_ASSERTION_FAILURE); - return (STAT_OK); + if ((cfg.magic_start != MAGICNUM) || (cfg.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; + if ((nvl.magic_start != MAGICNUM) || (nvl.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; + if ((nvStr.magic_start != MAGICNUM) || (nvStr.magic_end != MAGICNUM)) return STAT_CONFIG_ASSERTION_FAILURE; + if (global_string_buf[MESSAGE_LEN-1] != 0) return STAT_CONFIG_ASSERTION_FAILURE; + return STAT_OK; } /***** Generic Internal Functions *********************************************/ /* Generic gets() - * get_nul() - get nothing (returns STAT_PARAMETER_CANNOT_BE_READ) - * get_ui8() - get value as 8 bit uint8_t - * get_int() - get value as 32 bit integer - * get_data() - get value as 32 bit integer blind cast - * get_flt() - get value as float - * get_format() - internal accessor for printf() format string + * get_nul() - get nothing (returns STAT_PARAMETER_CANNOT_BE_READ) + * get_ui8() - get value as 8 bit uint8_t + * get_int() - get value as 32 bit integer + * get_data() - get value as 32 bit integer blind cast + * get_flt() - get value as float + * get_format() - internal accessor for printf() format string */ stat_t get_nul(nvObj_t *nv) { - nv->valuetype = TYPE_NULL; - return (STAT_PARAMETER_CANNOT_BE_READ); + nv->valuetype = TYPE_0; + return STAT_PARAMETER_CANNOT_BE_READ; } stat_t get_ui8(nvObj_t *nv) { - nv->value = (float)*((uint8_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); + nv->value = (float)*((uint8_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; } stat_t get_int(nvObj_t *nv) { -// nv->value = (float)*((uint32_t *)GET_TABLE_WORD(target)); - nv->value = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); + nv->value = *((uint32_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; } stat_t get_data(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - *v = *((uint32_t *)GET_TABLE_WORD(target)); - nv->valuetype = TYPE_DATA; - return (STAT_OK); + uint32_t *v = (uint32_t*)&nv->value; + *v = *((uint32_t *)GET_TABLE_WORD(target)); + nv->valuetype = TYPE_DATA; + return STAT_OK; } stat_t get_flt(nvObj_t *nv) { - nv->value = *((float *)GET_TABLE_WORD(target)); - nv->precision = (int8_t)GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return (STAT_OK); + nv->value = *((float *)GET_TABLE_WORD(target)); + nv->precision = (int8_t)GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return STAT_OK; } /* Generic sets() - * set_nul() - set nothing (returns STAT_PARAMETER_IS_READ_ONLY) - * set_ui8() - set value as 8 bit uint8_t value - * set_01() - set a 0 or 1 uint8_t value with validation - * set_012() - set a 0, 1 or 2 uint8_t value with validation - * set_0123() - set a 0, 1, 2 or 3 uint8_t value with validation - * set_int() - set value as 32 bit integer - * set_data() - set value as 32 bit integer blind cast - * set_flt() - set value as float + * set_nul() - set nothing (returns STAT_PARAMETER_IS_READ_ONLY) + * set_ui8() - set value as 8 bit uint8_t value + * set_01() - set a 0 or 1 uint8_t value with validation + * set_012() - set a 0, 1 or 2 uint8_t value with validation + * set_0123() - set a 0, 1, 2 or 3 uint8_t value with validation + * set_int() - set value as 32 bit integer + * set_data() - set value as 32 bit integer blind cast + * set_flt() - set value as float */ -stat_t set_nul(nvObj_t *nv) { return (STAT_PARAMETER_IS_READ_ONLY); } +stat_t set_nul(nvObj_t *nv) { return STAT_PARAMETER_IS_READ_ONLY; } stat_t set_ui8(nvObj_t *nv) { - *((uint8_t *)GET_TABLE_WORD(target)) = nv->value; - nv->valuetype = TYPE_INTEGER; - return(STAT_OK); + *((uint8_t *)GET_TABLE_WORD(target)) = nv->value; + nv->valuetype = TYPE_INTEGER; + return(STAT_OK); } stat_t set_01(nvObj_t *nv) { - if ((uint8_t)nv->value > 1) - return (STAT_INPUT_VALUE_RANGE_ERROR); // if - return (set_ui8(nv)); // else + if ((uint8_t)nv->value > 1) + return STAT_INPUT_VALUE_RANGE_ERROR; // if + return set_ui8(nv); // else } stat_t set_012(nvObj_t *nv) { - if ((uint8_t)nv->value > 2) - return (STAT_INPUT_VALUE_RANGE_ERROR); // if - return (set_ui8(nv)); // else + if ((uint8_t)nv->value > 2) + return STAT_INPUT_VALUE_RANGE_ERROR; // if + return set_ui8(nv); // else } stat_t set_0123(nvObj_t *nv) { - if ((uint8_t)nv->value > 3) - return (STAT_INPUT_VALUE_RANGE_ERROR); // if - return (set_ui8(nv)); // else + if ((uint8_t)nv->value > 3) + return STAT_INPUT_VALUE_RANGE_ERROR; // if + return set_ui8(nv); // else } stat_t set_int(nvObj_t *nv) { - *((uint32_t *)GET_TABLE_WORD(target)) = (uint32_t)nv->value; - nv->valuetype = TYPE_INTEGER; - return(STAT_OK); + *((uint32_t *)GET_TABLE_WORD(target)) = (uint32_t)nv->value; + nv->valuetype = TYPE_INTEGER; + return(STAT_OK); } stat_t set_data(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - *((uint32_t *)GET_TABLE_WORD(target)) = *v; - nv->valuetype = TYPE_DATA; - return(STAT_OK); + uint32_t *v = (uint32_t*)&nv->value; + *((uint32_t *)GET_TABLE_WORD(target)) = *v; + nv->valuetype = TYPE_DATA; + return(STAT_OK); } stat_t set_flt(nvObj_t *nv) { - *((float *)GET_TABLE_WORD(target)) = nv->value; - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return(STAT_OK); + *((float *)GET_TABLE_WORD(target)) = nv->value; + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + return(STAT_OK); } /************************************************************************************ * Group operations * - * Group operations work on parent/child groups where the parent is one of: - * axis group x,y,z,a,b,c - * motor group 1,2,3,4 - * PWM group p1 - * coordinate group g54,g55,g56,g57,g58,g59,g92 - * system group "sys" - a collection of otherwise unrelated variables + * Group operations work on parent/child groups where the parent is one of: + * axis group x,y,z,a,b,c + * motor group 1,2,3,4 + * PWM group p1 + * coordinate group g54,g55,g56,g57,g58,g59,g92 + * system group "sys" - a collection of otherwise unrelated variables * - * Text mode can only GET groups. For example: - * $x get all members of an axis group - * $1 get all members of a motor group - * $ get any named group from the above lists + * Text mode can only GET groups. For example: + * $x get all members of an axis group + * $1 get all members of a motor group + * $ get any named group from the above lists * - * In JSON groups are carried as parent / child objects & can get and set elements: - * {"x":""} get all X axis parameters - * {"x":{"vm":""}} get X axis velocity max - * {"x":{"vm":1000}} set X axis velocity max - * {"x":{"vm":"","fr":""}} get X axis velocity max and feed rate - * {"x":{"vm":1000,"fr";900}} set X axis velocity max and feed rate - * {"x":{"am":1,"fr":800,....}} set multiple or all X axis parameters + * In JSON groups are carried as parent / child objects & can get and set elements: + * {"x":""} get all X axis parameters + * {"x":{"vm":""}} get X axis velocity max + * {"x":{"vm":1000}} set X axis velocity max + * {"x":{"vm":"","fr":""}} get X axis velocity max and feed rate + * {"x":{"vm":1000,"fr";900}} set X axis velocity max and feed rate + * {"x":{"am":1,"fr":800,....}} set multiple or all X axis parameters */ /* * get_grp() - read data from axis, motor, system or other group * - * get_grp() is a group expansion function that expands the parent group and returns - * the values of all the children in that group. It expects the first nvObj in the - * nvBody to have a valid group name in the token field. This first object will be set - * to a TYPE_PARENT. The group field is left nul - as the group field refers to a parent - * group, which this group has none. + * get_grp() is a group expansion function that expands the parent group and returns + * the values of all the children in that group. It expects the first nvObj in the + * nvBody to have a valid group name in the token field. This first object will be set + * to a TYPE_PARENT. The group field is left nul - as the group field refers to a parent + * group, which this group has none. * - * All subsequent nvObjs in the body will be populated with their values. - * The token field will be populated as will the parent name in the group field. + * All subsequent nvObjs in the body will be populated with their values. + * The token field will be populated as will the parent name in the group field. * - * The sys group is an exception where the children carry a blank group field, even though - * the sys parent is labeled as a TYPE_PARENT. + * The sys group is an exception where the children carry a blank group field, even though + * the sys parent is labeled as a TYPE_PARENT. */ stat_t get_grp(nvObj_t *nv) { - char_t *parent_group = nv->token; // token in the parent nv object is the group - char_t group[GROUP_LEN+1]; // group string retrieved from cfgArray child - nv->valuetype = TYPE_PARENT; // make first object the parent - for (index_t i=0; nv_index_is_single(i); i++) { - strcpy_P(group, cfgArray[i].group); // don't need strncpy as it's always terminated - if (strcmp(parent_group, group) != 0) continue; - (++nv)->index = i; - nv_get_nvObj(nv); - } - return (STAT_OK); + char_t *parent_group = nv->token; // token in the parent nv object is the group + char_t group[GROUP_LEN+1]; // group string retrieved from cfgArray child + nv->valuetype = TYPE_PARENT; // make first object the parent + for (index_t i=0; nv_index_is_single(i); i++) { + strcpy_P(group, cfgArray[i].group); // don't need strncpy as it's always terminated + if (strcmp(parent_group, group) != 0) continue; + (++nv)->index = i; + nv_get_nvObj(nv); + } + return STAT_OK; } /* * set_grp() - get or set one or more values in a group * - * This functions is called "_set_group()" but technically it's a getter and - * a setter. It iterates the group children and either gets the value or sets - * the value for each depending on the nv->valuetype. + * This functions is called "_set_group()" but technically it's a getter and + * a setter. It iterates the group children and either gets the value or sets + * the value for each depending on the nv->valuetype. * - * This function serves JSON mode only as text mode shouldn't call it. + * This function serves JSON mode only as text mode shouldn't call it. */ stat_t set_grp(nvObj_t *nv) { - if (cfg.comm_mode == TEXT_MODE) - return (STAT_INVALID_OR_MALFORMED_COMMAND); + if (cfg.comm_mode == TEXT_MODE) + return STAT_INVALID_OR_MALFORMED_COMMAND; - for (uint8_t i=0; inx) == NULL) break; - if (nv->valuetype == TYPE_EMPTY) break; - else if (nv->valuetype == TYPE_NULL) // NULL means GET the value - nv_get(nv); - else { - nv_set(nv); - nv_persist(nv); - } - } - return (STAT_OK); + for (uint8_t i=0; inx) == 0) break; + if (nv->valuetype == TYPE_EMPTY) break; + else if (nv->valuetype == TYPE_0) // 0 means GET the value + nv_get(nv); + else { + nv_set(nv); + nv_persist(nv); + } + } + return STAT_OK; } /* * nv_group_is_prefixed() - hack * - * This little function deals with the exception cases that some groups don't use - * the parent token as a prefix to the child elements; SR being a good example. + * This little function deals with the exception cases that some groups don't use + * the parent token as a prefix to the child elements; SR being a good example. */ uint8_t nv_group_is_prefixed(char_t *group) { - if (strcmp("sr",group) == 0) return (false); - if (strcmp("sys",group) == 0) return (false); - return (true); + if (strcmp("sr",group) == 0) return false; + if (strcmp("sys",group) == 0) return false; + return true; } /*********************************************************************************** @@ -411,27 +406,27 @@ uint8_t nv_group_is_prefixed(char_t *group) */ index_t nv_get_index(const char_t *group, const char_t *token) { - char_t c; - char_t str[TOKEN_LEN + GROUP_LEN+1]; // should actually never be more than TOKEN_LEN+1 - strncpy(str, group, GROUP_LEN+1); - strncat(str, token, TOKEN_LEN+1); - - index_t i; - index_t index_max = nv_index_max(); - - for (i=0; i < index_max; i++) { - if ((c = GET_TOKEN_BYTE(token[0])) != str[0]) { continue; } // 1st character mismatch - if ((c = GET_TOKEN_BYTE(token[1])) == NUL) { if (str[1] == NUL) return(i);} // one character match - if (c != str[1]) continue; // 2nd character mismatch - if ((c = GET_TOKEN_BYTE(token[2])) == NUL) { if (str[2] == NUL) return(i);} // two character match - if (c != str[2]) continue; // 3rd character mismatch - if ((c = GET_TOKEN_BYTE(token[3])) == NUL) { if (str[3] == NUL) return(i);} // three character match - if (c != str[3]) continue; // 4th character mismatch - if ((c = GET_TOKEN_BYTE(token[4])) == NUL) { if (str[4] == NUL) return(i);} // four character match - if (c != str[4]) continue; // 5th character mismatch - return (i); // five character match - } - return (NO_MATCH); + char_t c; + char_t str[TOKEN_LEN + GROUP_LEN+1]; // should actually never be more than TOKEN_LEN+1 + strncpy(str, group, GROUP_LEN+1); + strncat(str, token, TOKEN_LEN+1); + + index_t i; + index_t index_max = nv_index_max(); + + for (i=0; i < index_max; i++) { + if ((c = GET_TOKEN_BYTE(token[0])) != str[0]) { continue; } // 1st character mismatch + if ((c = GET_TOKEN_BYTE(token[1])) == 0) { if (str[1] == 0) return(i);} // one character match + if (c != str[1]) continue; // 2nd character mismatch + if ((c = GET_TOKEN_BYTE(token[2])) == 0) { if (str[2] == 0) return(i);} // two character match + if (c != str[2]) continue; // 3rd character mismatch + if ((c = GET_TOKEN_BYTE(token[3])) == 0) { if (str[3] == 0) return(i);} // three character match + if (c != str[3]) continue; // 4th character mismatch + if ((c = GET_TOKEN_BYTE(token[4])) == 0) { if (str[4] == 0) return(i);} // four character match + if (c != str[4]) continue; // 5th character mismatch + return i; // five character match + } + return NO_MATCH; } /* @@ -440,251 +435,242 @@ index_t nv_get_index(const char_t *group, const char_t *token) uint8_t nv_get_type(nvObj_t *nv) { - if (nv->token[0] == NUL) return (NV_TYPE_NULL); - if (strcmp("gc", nv->token) == 0) return (NV_TYPE_GCODE); - if (strcmp("sr", nv->token) == 0) return (NV_TYPE_REPORT); - if (strcmp("qr", nv->token) == 0) return (NV_TYPE_REPORT); - if (strcmp("msg",nv->token) == 0) return (NV_TYPE_MESSAGE); - if (strcmp("err",nv->token) == 0) return (NV_TYPE_MESSAGE); // errors are reported as messages - if (strcmp("n", nv->token) == 0) return (NV_TYPE_LINENUM); - return (NV_TYPE_CONFIG); + if (nv->token[0] == 0) return NV_TYPE_0; + if (strcmp("gc", nv->token) == 0) return NV_TYPE_GCODE; + if (strcmp("sr", nv->token) == 0) return NV_TYPE_REPORT; + if (strcmp("qr", nv->token) == 0) return NV_TYPE_REPORT; + if (strcmp("msg",nv->token) == 0) return NV_TYPE_MESSAGE; + if (strcmp("err",nv->token) == 0) return NV_TYPE_MESSAGE; // errors are reported as messages + if (strcmp("n", nv->token) == 0) return NV_TYPE_LINENUM; + return NV_TYPE_CONFIG; } /****************************************************************************** * nvObj low-level object and list operations - * nv_get_nvObj() - setup a nv object by providing the index - * nv_reset_nv() - quick clear for a new nv object - * nv_reset_nv_list() - clear entire header, body and footer for a new use - * nv_copy_string() - used to write a string to shared string storage and link it - * nv_add_object() - write contents of parameter to first free object in the body - * nv_add_integer() - add an integer value to end of nv body (Note 1) - * nv_add_float() - add a floating point value to end of nv body - * nv_add_string() - add a string object to end of nv body + * nv_get_nvObj() - setup a nv object by providing the index + * nv_reset_nv() - quick clear for a new nv object + * nv_reset_nv_list() - clear entire header, body and footer for a new use + * nv_copy_string() - used to write a string to shared string storage and link it + * nv_add_object() - write contents of parameter to first free object in the body + * nv_add_integer() - add an integer value to end of nv body (Note 1) + * nv_add_float() - add a floating point value to end of nv body + * nv_add_string() - add a string object to end of nv body * nv_add_conditional_message() - add a message to nv body if messages are enabled * - * Note: Functions that return a nv pointer point to the object that was modified or - * a NULL pointer if there was an error. + * Note: Functions that return a nv pointer point to the object that was modified or + * a 0 pointer if there was an error. * - * Note: Adding a really large integer (like a checksum value) may lose precision due - * to the cast to a float. Sometimes it's better to load an integer as a string if - * all you want to do is display it. + * Note: Adding a really large integer (like a checksum value) may lose precision due + * to the cast to a float. Sometimes it's better to load an integer as a string if + * all you want to do is display it. * - * Note: A trick is to cast all string constants for nv_copy_string(), nv_add_object(), - * nv_add_string() and nv_add_conditional_message() to (const char_t *). Examples: + * Note: A trick is to cast all string constants for nv_copy_string(), nv_add_object(), + * nv_add_string() and nv_add_conditional_message() to (const char_t *). Examples: * - * nv_add_string((const char_t *)"msg", string); + * nv_add_string((const char_t *)"msg", string); * - * On the AVR this will save a little static RAM. The "msg" string will occupy flash - * as an initializer and be instantiated in stack RAM when the function executes. + * On the AVR this will save a little static RAM. The "msg" string will occupy flash + * as an initializer and be instantiated in stack RAM when the function executes. */ void nv_get_nvObj(nvObj_t *nv) { - if (nv->index >= nv_index_max()) { return; } // sanity - - index_t tmp = nv->index; - nv_reset_nv(nv); - nv->index = tmp; - - strcpy_P(nv->token, cfgArray[nv->index].token); // token field is always terminated - strcpy_P(nv->group, cfgArray[nv->index].group); // group field is always terminated - - // special processing for system groups and stripping tokens for groups - if (nv->group[0] != NUL) { - if (GET_TABLE_BYTE(flags) & F_NOSTRIP) { - nv->group[0] = NUL; - } else { - strcpy(nv->token, &nv->token[strlen(nv->group)]); // strip group from the token - } - } - ((fptrCmd)GET_TABLE_WORD(get))(nv); // populate the value -} - -nvObj_t *nv_reset_nv(nvObj_t *nv) // clear a single nvObj structure -{ - nv->valuetype = TYPE_EMPTY; // selective clear is much faster than calling memset - nv->index = 0; - nv->value = 0; - nv->precision = 0; - nv->token[0] = NUL; - nv->group[0] = NUL; - nv->stringp = NULL; - - if (nv->pv == NULL) { // set depth correctly - nv->depth = 0; - } else { - if (nv->pv->valuetype == TYPE_PARENT) { - nv->depth = nv->pv->depth + 1; - } else { - nv->depth = nv->pv->depth; - } - } - return (nv); // return pointer to nv as a convenience to callers -} - -nvObj_t *nv_reset_nv_list() // clear the header and response body -{ - nvStr.wp = 0; // reset the shared string - nvObj_t *nv = nvl.list; // set up linked list and initialize elements - for (uint8_t i=0; ipv = (nv-1); // the ends are bogus & corrected later - nv->nx = (nv+1); - nv->index = 0; - nv->depth = 1; // header and footer are corrected later - nv->precision = 0; - nv->valuetype = TYPE_EMPTY; - nv->token[0] = NUL; - } - (--nv)->nx = NULL; - nv = nvl.list; // setup response header element ('r') - nv->pv = NULL; - nv->depth = 0; - nv->valuetype = TYPE_PARENT; - strcpy(nv->token, "r"); - return (nv_body); // this is a convenience for calling routines + if (nv->index >= nv_index_max()) { return; } // sanity + + index_t tmp = nv->index; + nv_reset_nv(nv); + nv->index = tmp; + + strcpy_P(nv->token, cfgArray[nv->index].token); // token field is always terminated + strcpy_P(nv->group, cfgArray[nv->index].group); // group field is always terminated + + // special processing for system groups and stripping tokens for groups + if (nv->group[0] != 0) { + if (GET_TABLE_BYTE(flags) & F_NOSTRIP) { + nv->group[0] = 0; + } else { + strcpy(nv->token, &nv->token[strlen(nv->group)]); // strip group from the token + } + } + ((fptrCmd)GET_TABLE_WORD(get))(nv); // populate the value +} + +nvObj_t *nv_reset_nv(nvObj_t *nv) // clear a single nvObj structure +{ + nv->valuetype = TYPE_EMPTY; // selective clear is much faster than calling memset + nv->index = 0; + nv->value = 0; + nv->precision = 0; + nv->token[0] = 0; + nv->group[0] = 0; + nv->stringp = 0; + + if (nv->pv == 0) { // set depth correctly + nv->depth = 0; + } else { + if (nv->pv->valuetype == TYPE_PARENT) { + nv->depth = nv->pv->depth + 1; + } else { + nv->depth = nv->pv->depth; + } + } + return nv; // return pointer to nv as a convenience to callers +} + +nvObj_t *nv_reset_nv_list() // clear the header and response body +{ + nvStr.wp = 0; // reset the shared string + nvObj_t *nv = nvl.list; // set up linked list and initialize elements + for (uint8_t i=0; ipv = (nv-1); // the ends are bogus & corrected later + nv->nx = (nv+1); + nv->index = 0; + nv->depth = 1; // header and footer are corrected later + nv->precision = 0; + nv->valuetype = TYPE_EMPTY; + nv->token[0] = 0; + } + (--nv)->nx = 0; + nv = nvl.list; // setup response header element ('r') + nv->pv = 0; + nv->depth = 0; + nv->valuetype = TYPE_PARENT; + strcpy(nv->token, "r"); + return nv_body; // this is a convenience for calling routines } stat_t nv_copy_string(nvObj_t *nv, const char_t *src) { - if ((nvStr.wp + strlen(src)) > NV_SHARED_STRING_LEN) - return (STAT_BUFFER_FULL); + if ((nvStr.wp + strlen(src)) > NV_SHARED_STRING_LEN) + return STAT_BUFFER_FULL; - char_t *dst = &nvStr.string[nvStr.wp]; - strcpy(dst, src); // copy string to current head position - // string has already been tested for overflow, above - nvStr.wp += strlen(src)+1; // advance head for next string - nv->stringp = (char_t (*)[])dst; - return (STAT_OK); -} - -/* UNUSED -stat_t nv_copy_string_P(nvObj_t *nv, const char_t *src_P) -{ - char_t buf[NV_SHARED_STRING_LEN]; - strncpy_P(buf, src_P, NV_SHARED_STRING_LEN); - return (nv_copy_string(nv, buf)); + char_t *dst = &nvStr.string[nvStr.wp]; + strcpy(dst, src); // copy string to current head position + // string has already been tested for overflow, above + nvStr.wp += strlen(src)+1; // advance head for next string + nv->stringp = (char_t (*)[])dst; + return STAT_OK; } -*/ nvObj_t *nv_add_object(const char_t *token) // add an object to the body using a token { - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == NULL) return(NULL); // not supposed to find a NULL; here for safety - continue; - } - // load the index from the token or die trying - if ((nv->index = nv_get_index((const char_t *)"",token)) == NO_MATCH) { return (NULL);} - nv_get_nvObj(nv); // populate the object from the index - return (nv); - } - return (NULL); + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; + } + // load the index from the token or die trying + if ((nv->index = nv_get_index((const char_t *)"",token)) == NO_MATCH) { return 0;} + nv_get_nvObj(nv); // populate the object from the index + return nv; + } + return 0; } nvObj_t *nv_add_integer(const char_t *token, const uint32_t value)// add an integer object to the body { - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == NULL) return(NULL); // not supposed to find a NULL; here for safety - continue; - } - strncpy(nv->token, token, TOKEN_LEN); - nv->value = (float) value; - nv->valuetype = TYPE_INTEGER; - return (nv); - } - return (NULL); + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; + } + strncpy(nv->token, token, TOKEN_LEN); + nv->value = (float) value; + nv->valuetype = TYPE_INTEGER; + return nv; + } + return 0; } nvObj_t *nv_add_data(const char_t *token, const uint32_t value)// add an integer object to the body { - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == NULL) return(NULL); // not supposed to find a NULL; here for safety - continue; - } - strcpy(nv->token, token); - float *v = (float*)&value; - nv->value = *v; - nv->valuetype = TYPE_DATA; - return (nv); - } - return (NULL); -} - -nvObj_t *nv_add_float(const char_t *token, const float value) // add a float object to the body -{ - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == NULL) return(NULL); // not supposed to find a NULL; here for safety - continue; - } - strncpy(nv->token, token, TOKEN_LEN); - nv->value = value; - nv->valuetype = TYPE_FLOAT; - return (nv); - } - return (NULL); + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; + } + strcpy(nv->token, token); + float *v = (float*)&value; + nv->value = *v; + nv->valuetype = TYPE_DATA; + return nv; + } + return 0; +} + +nvObj_t *nv_add_float(const char_t *token, const float value) // add a float object to the body +{ + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; + } + strncpy(nv->token, token, TOKEN_LEN); + nv->value = value; + nv->valuetype = TYPE_FLOAT; + return nv; + } + return 0; } // ASSUMES A RAM STRING. If you need to post a FLASH string use pstr2str to convert it to a RAM string nvObj_t *nv_add_string(const char_t *token, const char_t *string) // add a string object to the body { - nvObj_t *nv = nv_body; - for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { - if ((nv = nv->nx) == NULL) return(NULL); // not supposed to find a NULL; here for safety - continue; - } - strncpy(nv->token, token, TOKEN_LEN); - if (nv_copy_string(nv, string) != STAT_OK) - return (NULL); + nvObj_t *nv = nv_body; + for (uint8_t i=0; ivaluetype != TYPE_EMPTY) { + if ((nv = nv->nx) == 0) return(0); // not supposed to find a 0; here for safety + continue; + } + strncpy(nv->token, token, TOKEN_LEN); + if (nv_copy_string(nv, string) != STAT_OK) + return 0; - nv->index = nv_get_index((const char_t *)"", nv->token); - nv->valuetype = TYPE_STRING; - return (nv); - } - return (NULL); + nv->index = nv_get_index((const char_t *)"", nv->token); + nv->valuetype = TYPE_STRING; + return nv; + } + return 0; } /* * cm_conditional_message() - queue a RAM string as a message in the response (conditionally) * - * Note: If you need to post a FLASH string use pstr2str to convert it to a RAM string + * Note: If you need to post a FLASH string use pstr2str to convert it to a RAM string */ -nvObj_t *nv_add_conditional_message(const char_t *string) // conditionally add a message object to the body +nvObj_t *nv_add_conditional_message(const char_t *string) // conditionally add a message object to the body { - if ((cfg.comm_mode == JSON_MODE) && (js.echo_json_messages != true)) { return (NULL);} - return(nv_add_string((const char_t *)"msg", string)); + if ((cfg.comm_mode == JSON_MODE) && (js.echo_json_messages != true)) { return 0;} + return(nv_add_string((const char_t *)"msg", string)); } /**** nv_print_list() - print nv_array as JSON or text ********************** * - * Generate and print the JSON and text mode output strings. Use this function - * for all text and JSON output that wants to be in a response header. - * Don't just printf stuff. + * Generate and print the JSON and text mode output strings. Use this function + * for all text and JSON output that wants to be in a response header. + * Don't just printf stuff. * - * Inputs: - * json_flags = JSON_OBJECT_FORMAT - print just the body w/o header or footer - * json_flags = JSON_RESPONSE_FORMAT - print a full "r" object with footer + * Inputs: + * json_flags = JSON_OBJECT_FORMAT - print just the body w/o header or footer + * json_flags = JSON_RESPONSE_FORMAT - print a full "r" object with footer * - * text_flags = TEXT_INLINE_PAIRS - print text as name/value pairs on a single line - * text_flags = TEXT_INLINE_VALUES - print text as comma separated values on a single line - * text_flags = TEXT_MULTILINE_FORMATTED - print text one value per line with formatting string + * text_flags = TEXT_INLINE_PAIRS - print text as name/value pairs on a single line + * text_flags = TEXT_INLINE_VALUES - print text as comma separated values on a single line + * text_flags = TEXT_MULTILINE_FORMATTED - print text one value per line with formatting string */ void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags) { - if (cfg.comm_mode == JSON_MODE) { - json_print_list(status, json_flags); - } else { - text_print_list(status, text_flags); - } + if (cfg.comm_mode == JSON_MODE) { + json_print_list(status, json_flags); + } else { + text_print_list(status, text_flags); + } } /**************************************************************************** @@ -693,13 +679,13 @@ void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags) void nv_dump_nv(nvObj_t *nv) { - printf ("i:%d, d:%d, t:%d, p:%d, v:%f, g:%s, t:%s, s:%s\n", - nv->index, - nv->depth, - nv->valuetype, - nv->precision, - (double)nv->value, - nv->group, - nv->token, - (char *)nv->stringp); + printf ("i:%d, d:%d, t:%d, p:%d, v:%f, g:%s, t:%s, s:%s\n", + nv->index, + nv->depth, + nv->valuetype, + nv->precision, + (double)nv->value, + nv->group, + nv->token, + (char *)nv->stringp); } diff --git a/src/config.h b/src/config.h index 1ec7da8..2d80ff9 100755 --- a/src/config.h +++ b/src/config.h @@ -29,269 +29,269 @@ #define CONFIG_H_ONCE /***** PLEASE NOTE ***** -#include "config_app.h" // is present at the end of this file +#include "config_app.h" // is present at the end of this file */ /**** Config System Overview and Usage *** * - * --- Config objects and the config list --- + * --- Config objects and the config list --- * - * The config system provides a structured way to get, set and print configuration variables. - * The config system operates as a list of "objects" (OK, so they are not really objects) - * that encapsulate each variable. The list may also may have header and footer objects. + * The config system provides a structured way to get, set and print configuration variables. + * The config system operates as a list of "objects" (OK, so they are not really objects) + * that encapsulate each variable. The list may also may have header and footer objects. * - * The list is populated by the text_parser or the JSON_parser depending on the mode. - * This way the internals don't care about how the variable is represented or communicated - * externally as all internal operations occur on the nvObjs, not the wire form (text or JSON). + * The list is populated by the text_parser or the JSON_parser depending on the mode. + * This way the internals don't care about how the variable is represented or communicated + * externally as all internal operations occur on the nvObjs, not the wire form (text or JSON). */ -/* --- Config variables, tables and strings --- +/* --- Config variables, tables and strings --- * - * Each configuration value is identified by a short mnemonic string (token). The token - * is resolved to an index into the cfgArray which is an array of structures with the - * static assignments for each variable. The cfgArray contains typed data in program - * memory (PROGMEM, in the AVR). + * Each configuration value is identified by a short mnemonic string (token). The token + * is resolved to an index into the cfgArray which is an array of structures with the + * static assignments for each variable. The cfgArray contains typed data in program + * memory (PROGMEM, in the AVR). * - * Each cfgItem has: - * - group string identifying what group the variable is part of; or "" if no group - * - token string - the token for that variable - pre-pended with the group (if present) - * - operations flags - e.g. if the value should be initialized and/or persisted to NVM - * - function pointer for formatted print() method for text-mode readouts - * - function pointer for get() method - gets value from memory - * - function pointer for set() method - sets value and runs functions - * - target - memory location that the value is written to / read from - * - default value - for cold initialization + * Each cfgItem has: + * - group string identifying what group the variable is part of; or "" if no group + * - token string - the token for that variable - pre-pended with the group (if present) + * - operations flags - e.g. if the value should be initialized and/or persisted to NVM + * - function pointer for formatted print() method for text-mode readouts + * - function pointer for get() method - gets value from memory + * - function pointer for set() method - sets value and runs functions + * - target - memory location that the value is written to / read from + * - default value - for cold initialization * - * Additionally an NVM array contains values persisted to EEPROM as floats; indexed by cfgArray index + * Additionally an NVM array contains values persisted to EEPROM as floats; indexed by cfgArray index * - * The following rules apply to mnemonic tokens - * - are up to 5 alphnuneric characters and cannot contain whitespace or separators - * - must be unique (non colliding). - * - axis tokens start with the axis letter and are typically 3 characters including the axis letter - * - motor tokens start with the motor digit and are typically 3 characters including the motor digit - * - non-axis or non-motor tokens are 2-5 characters and by convention generally should not start - * with: xyzabcuvw0123456789 (but there can be exceptions) + * The following rules apply to mnemonic tokens + * - are up to 5 alphnuneric characters and cannot contain whitespace or separators + * - must be unique (non colliding). + * - axis tokens start with the axis letter and are typically 3 characters including the axis letter + * - motor tokens start with the motor digit and are typically 3 characters including the motor digit + * - non-axis or non-motor tokens are 2-5 characters and by convention generally should not start + * with: xyzabcuvw0123456789 (but there can be exceptions) * * "Groups" are collections of values that mimic REST resources. Groups include: - * - axis groups prefixed by "xyzabc" ("uvw" are reserved) - * - motor groups prefixed by "1234" ("56789" are reserved) - * - PWM groups prefixed by p1, p2 (p3 - p9 are reserved) - * - coordinate system groups prefixed by g54, g55, g56, g57, g59, g92 - * - a system group is identified by "sys" and contains a collection of otherwise unrelated values + * - axis groups prefixed by "xyzabc" ("uvw" are reserved) + * - motor groups prefixed by "1234" ("56789" are reserved) + * - PWM groups prefixed by p1, p2 (p3 - p9 are reserved) + * - coordinate system groups prefixed by g54, g55, g56, g57, g59, g92 + * - a system group is identified by "sys" and contains a collection of otherwise unrelated values * - * "Uber-groups" are groups of groups that are only used for text-mode printing - e.g. - * - group of all axes groups - * - group of all motor groups - * - group of all offset groups - * - group of all groups + * "Uber-groups" are groups of groups that are only used for text-mode printing - e.g. + * - group of all axes groups + * - group of all motor groups + * - group of all offset groups + * - group of all groups */ /* --- Making changes and adding new values * - * Adding a new value to config (or changing an existing one) involves touching the following places: + * Adding a new value to config (or changing an existing one) involves touching the following places: * - * - Create a new record in cfgArray[]. Use existing ones for examples. + * - Create a new record in cfgArray[]. Use existing ones for examples. * - * - Create functions for print, get, and set. You can often use existing generic fucntions for - * get and set, and sometimes print. If print requires any custom text it requires it's own function - * Look in the modules for examples - e.g. at the end of canoonical_machine.c + * - Create functions for print, get, and set. You can often use existing generic fucntions for + * get and set, and sometimes print. If print requires any custom text it requires it's own function + * Look in the modules for examples - e.g. at the end of canoonical_machine.c * - * - The ordering of group displays is set by the order of items in cfgArray. None of the other - * orders matter but are generally kept sequenced for easier reading and code maintenance. Also, - * Items earlier in the array will resolve token searches faster than ones later in the array. + * - The ordering of group displays is set by the order of items in cfgArray. None of the other + * orders matter but are generally kept sequenced for easier reading and code maintenance. Also, + * Items earlier in the array will resolve token searches faster than ones later in the array. * - * Note that matching will occur from the most specific to the least specific, meaning that - * if tokens overlap the longer one should be earlier in the array: "gco" should precede "gc". + * Note that matching will occur from the most specific to the least specific, meaning that + * if tokens overlap the longer one should be earlier in the array: "gco" should precede "gc". */ /**** nvObj lists **** * - * Commands and groups of commands are processed internally a doubly linked list of nvObj_t - * structures. This isolates the command and config internals from the details of communications, - * parsing and display in text mode and JSON mode. + * Commands and groups of commands are processed internally a doubly linked list of nvObj_t + * structures. This isolates the command and config internals from the details of communications, + * parsing and display in text mode and JSON mode. * - * The first element of the list is designated the response header element ("r") but the list - * can also be serialized as a simple object by skipping over the header + * The first element of the list is designated the response header element ("r") but the list + * can also be serialized as a simple object by skipping over the header * - * To use the nvObj list first reset it by calling nv_reset_nv_list(). This initializes the - * header, marks the the objects as TYPE_EMPTY (-1), resets the shared string, relinks all objects - * with NX and PV pointers, and makes the last element the terminating element by setting its NX - * pointer to NULL. The terminating element may carry data, and will be processed. + * To use the nvObj list first reset it by calling nv_reset_nv_list(). This initializes the + * header, marks the the objects as TYPE_EMPTY (-1), resets the shared string, relinks all objects + * with NX and PV pointers, and makes the last element the terminating element by setting its NX + * pointer to 0. The terminating element may carry data, and will be processed. * - * When you use the list you can terminate your own last element, or just leave the EMPTY elements - * to be skipped over during output serialization. + * When you use the list you can terminate your own last element, or just leave the EMPTY elements + * to be skipped over during output serialization. * - * We don't use recursion so parent/child nesting relationships are captured in a 'depth' variable, - * This must remain consistent if the curlies are to work out. You should not have to track depth - * explicitly if you use nv_reset_nv_list() or the accessor functions like nv_add_integer() or - * nv_add_message(). If you see problems with curlies check the depth values in the lists. + * We don't use recursion so parent/child nesting relationships are captured in a 'depth' variable, + * This must remain consistent if the curlies are to work out. You should not have to track depth + * explicitly if you use nv_reset_nv_list() or the accessor functions like nv_add_integer() or + * nv_add_message(). If you see problems with curlies check the depth values in the lists. * - * Use the nv_print_list() dispatcher for all JSON and text output. Do not simply run through printf. + * Use the nv_print_list() dispatcher for all JSON and text output. Do not simply run through printf. */ -/* Token and Group Fields +/* Token and Group Fields * - * The nvObject struct (nvObj_t) has strict rules on the use of the token and group fields. - * The following forms are legal which support the use cases listed: + * The nvObject struct (nvObj_t) has strict rules on the use of the token and group fields. + * The following forms are legal which support the use cases listed: * - * Forms - * - group is NUL; token is full token including any group profix - * - group is populated; token is carried without the group prefix - * - group is populated; token is NUL - indicates a group operation + * Forms + * - group is 0; token is full token including any group profix + * - group is populated; token is carried without the group prefix + * - group is populated; token is 0 - indicates a group operation * * Use Cases - * - Lookup full token in cfgArray to get the index. Concatenates grp+token as key - * - Text-mode displays. Concatenates grp+token for display, may also use grp alone - * - JSON-mode display for single - element value e.g. xvm. Concatenate as above - * - JSON-mode display of a parent/child group. Parent is named grp, children nems are tokens + * - Lookup full token in cfgArray to get the index. Concatenates grp+token as key + * - Text-mode displays. Concatenates grp+token for display, may also use grp alone + * - JSON-mode display for single - element value e.g. xvm. Concatenate as above + * - JSON-mode display of a parent/child group. Parent is named grp, children nems are tokens */ -/* --- nv object string handling --- +/* --- nv object string handling --- * - * It's very expensive to allocate sufficient string space to each nvObj, so nv uses a cheater's - * malloc. A single string of length NV_SHARED_STRING_LEN is shared by all nvObjs for all strings. - * The observation is that the total rendered output in JSON or text mode cannot exceed the size of - * the output buffer (typ 256 bytes), So some number less than that is sufficient for shared strings. - * This is all mediated through nv_copy_string(), nv_copy_string_P(), and nv_reset_nv_list(). + * It's very expensive to allocate sufficient string space to each nvObj, so nv uses a cheater's + * malloc. A single string of length NV_SHARED_STRING_LEN is shared by all nvObjs for all strings. + * The observation is that the total rendered output in JSON or text mode cannot exceed the size of + * the output buffer (typ 256 bytes), So some number less than that is sufficient for shared strings. + * This is all mediated through nv_copy_string(), nv_copy_string_P(), and nv_reset_nv_list(). */ /* --- Setting nvObj indexes --- * - * It's the responsibility of the object creator to set the index. Downstream functions - * all expect a valid index. Set the index by calling nv_get_index(). This also validates - * the token and group if no lookup exists. Setting the index is an expensive operation - * (linear table scan), so there are some exceptions where the index does not need to be set. - * These cases are put in the code, commented out, and explained. + * It's the responsibility of the object creator to set the index. Downstream functions + * all expect a valid index. Set the index by calling nv_get_index(). This also validates + * the token and group if no lookup exists. Setting the index is an expensive operation + * (linear table scan), so there are some exceptions where the index does not need to be set. + * These cases are put in the code, commented out, and explained. */ -/* --- Other Notes:--- +/* --- Other Notes:--- * - * NV_BODY_LEN needs to allow for one parent JSON object and enough children to complete the - * largest possible operation - usually the status report. + * NV_BODY_LEN needs to allow for one parent JSON object and enough children to complete the + * largest possible operation - usually the status report. */ /*********************************************************************************** **** DEFINITIONS AND SETTINGS ***************************************************** ***********************************************************************************/ -// Sizing and footprints // chose one based on # of elements in cfgArray -//typedef uint8_t index_t; // use this if there are < 256 indexed objects -typedef uint16_t index_t; // use this if there are > 255 indexed objects +// Sizing and footprints // chose one based on # of elements in cfgArray +//typedef uint8_t index_t; // use this if there are < 256 indexed objects +typedef uint16_t index_t; // use this if there are > 255 indexed objects - // defines allocated from stack (not-pre-allocated) -#define NV_FORMAT_LEN 128 // print formatting string max length -#define NV_MESSAGE_LEN 128 // sufficient space to contain end-user messages + // defines allocated from stack (not-pre-allocated) +#define NV_FORMAT_LEN 128 // print formatting string max length +#define NV_MESSAGE_LEN 128 // sufficient space to contain end-user messages - // pre-allocated defines (take RAM permanently) -#define NV_SHARED_STRING_LEN 512 // shared string for string values -#define NV_BODY_LEN 30 // body elements - allow for 1 parent + N children - // (each body element takes about 30 bytes of RAM) + // pre-allocated defines (take RAM permanently) +#define NV_SHARED_STRING_LEN 512 // shared string for string values +#define NV_BODY_LEN 30 // body elements - allow for 1 parent + N children + // (each body element takes about 30 bytes of RAM) // Stuff you probably don't want to change -#define GROUP_LEN 3 // max length of group prefix -#define TOKEN_LEN 5 // mnemonic token string: group prefix + short token -#define NV_FOOTER_LEN 18 // sufficient space to contain a JSON footer array -#define NV_LIST_LEN (NV_BODY_LEN+2) // +2 allows for a header and a footer -#define NV_MAX_OBJECTS (NV_BODY_LEN-1) // maximum number of objects in a body string +#define GROUP_LEN 3 // max length of group prefix +#define TOKEN_LEN 5 // mnemonic token string: group prefix + short token +#define NV_FOOTER_LEN 18 // sufficient space to contain a JSON footer array +#define NV_LIST_LEN (NV_BODY_LEN+2) // +2 allows for a header and a footer +#define NV_MAX_OBJECTS (NV_BODY_LEN-1) // maximum number of objects in a body string #define NO_MATCH (index_t)0xFFFF #define NV_STATUS_REPORT_LEN NV_MAX_OBJECTS // max number of status report elements - see cfgArray - // **** must also line up in cfgArray, se00 - seXX **** + // **** must also line up in cfgArray, se00 - seXX **** enum tgCommunicationsMode { - TEXT_MODE = 0, // text command line mode - JSON_MODE, // strict JSON construction - JSON_MODE_RELAXED // relaxed JSON construction (future) + TEXT_MODE = 0, // text command line mode + JSON_MODE, // strict JSON construction + JSON_MODE_RELAXED // relaxed JSON construction (future) }; enum flowControl { - FLOW_CONTROL_OFF = 0, // flow control disabled - FLOW_CONTROL_XON, // flow control uses XON/XOFF - FLOW_CONTROL_RTS // flow control uses RTS/CTS + FLOW_CONTROL_OFF = 0, // flow control disabled + FLOW_CONTROL_XON, // flow control uses XON/XOFF + FLOW_CONTROL_RTS // flow control uses RTS/CTS }; /* -enum lineTermination { // REMOVED. Too easy to make the board non-responsive (not a total brick, but close) - IGNORE_OFF = 0, // accept either CR or LF as termination on RX text line - IGNORE_CR, // ignore CR on RX - IGNORE_LF // ignore LF on RX +enum lineTermination { // REMOVED. Too easy to make the board non-responsive (not a total brick, but close) + IGNORE_OFF = 0, // accept either CR or LF as termination on RX text line + IGNORE_CR, // ignore CR on RX + IGNORE_LF // ignore LF on RX }; */ /* enum tgCommunicationsSticky { - NOT_STICKY = 0, // communications mode changes automatically - STICKY // communications mode does not change + NOT_STICKY = 0, // communications mode changes automatically + STICKY // communications mode does not change }; */ -enum valueType { // value typing for config and JSON - TYPE_EMPTY = -1, // value struct is empty (which is not the same as "NULL") - TYPE_NULL = 0, // value is 'null' (meaning the JSON null value) - TYPE_BOOL, // value is "true" (1) or "false"(0) - TYPE_INTEGER, // value is a uint32_t - TYPE_DATA, // value is blind cast to uint32_t - TYPE_FLOAT, // value is a floating point number - TYPE_STRING, // value is in string field - TYPE_ARRAY, // value is array element count, values are CSV ASCII in string field - TYPE_PARENT // object is a parent to a sub-object +enum valueType { // value typing for config and JSON + TYPE_EMPTY = -1, // value struct is empty (which is not the same as "0") + TYPE_0 = 0, // value is 'null' (meaning the JSON null value) + TYPE_BOOL, // value is "true" (1) or "false"(0) + TYPE_INTEGER, // value is a uint32_t + TYPE_DATA, // value is blind cast to uint32_t + TYPE_FLOAT, // value is a floating point number + TYPE_STRING, // value is in string field + TYPE_ARRAY, // value is array element count, values are CSV ASCII in string field + TYPE_PARENT // object is a parent to a sub-object }; /**** operations flags and shorthand ****/ -#define F_INITIALIZE 0x01 // initialize this item (run set during initialization) -#define F_PERSIST 0x02 // persist this item when set is run -#define F_NOSTRIP 0x04 // do not strip the group prefix from the token -#define F_CONVERT 0x08 // set if unit conversion is required - -#define _f0 0x00 -#define _fi (F_INITIALIZE) -#define _fp (F_PERSIST) -#define _fn (F_NOSTRIP) -#define _fc (F_CONVERT) -#define _fip (F_INITIALIZE | F_PERSIST) -#define _fipc (F_INITIALIZE | F_PERSIST | F_CONVERT) -#define _fipn (F_INITIALIZE | F_PERSIST | F_NOSTRIP) -#define _fipnc (F_INITIALIZE | F_PERSIST | F_NOSTRIP | F_CONVERT) +#define F_INITIALIZE 0x01 // initialize this item (run set during initialization) +#define F_PERSIST 0x02 // persist this item when set is run +#define F_NOSTRIP 0x04 // do not strip the group prefix from the token +#define F_CONVERT 0x08 // set if unit conversion is required + +#define _f0 0x00 +#define _fi (F_INITIALIZE) +#define _fp (F_PERSIST) +#define _fn (F_NOSTRIP) +#define _fc (F_CONVERT) +#define _fip (F_INITIALIZE | F_PERSIST) +#define _fipc (F_INITIALIZE | F_PERSIST | F_CONVERT) +#define _fipn (F_INITIALIZE | F_PERSIST | F_NOSTRIP) +#define _fipnc (F_INITIALIZE | F_PERSIST | F_NOSTRIP | F_CONVERT) /**** Structures ****/ -typedef struct nvString { // shared string object - uint16_t magic_start; +typedef struct nvString { // shared string object + uint16_t magic_start; #if (NV_SHARED_STRING_LEN < 256) - uint8_t wp; // use this string array index value if string len < 256 bytes + uint8_t wp; // use this string array index value if string len < 256 bytes #else - uint16_t wp; // use this string array index value is string len > 255 bytes + uint16_t wp; // use this string array index value is string len > 255 bytes #endif - char_t string[NV_SHARED_STRING_LEN]; - uint16_t magic_end; // guard to detect string buffer underruns + char_t string[NV_SHARED_STRING_LEN]; + uint16_t magic_end; // guard to detect string buffer underruns } nvStr_t; -typedef struct nvObject { // depending on use, not all elements may be populated - struct nvObject *pv; // pointer to previous object or NULL if first object - struct nvObject *nx; // pointer to next object or NULL if last object - index_t index; // index of tokenized name, or -1 if no token (optional) - int8_t depth; // depth of object in the tree. 0 is root (-1 is invalid) - int8_t valuetype; // see valueType enum - int8_t precision; // decimal precision for reporting (JSON) - float value; // numeric value - char_t group[GROUP_LEN+1]; // group prefix or NUL if not in a group - char_t token[TOKEN_LEN+1]; // full mnemonic token for lookup - char_t (*stringp)[]; // pointer to array of characters from shared character array -} nvObj_t; // OK, so it's not REALLY an object +typedef struct nvObject { // depending on use, not all elements may be populated + struct nvObject *pv; // pointer to previous object or 0 if first object + struct nvObject *nx; // pointer to next object or 0 if last object + index_t index; // index of tokenized name, or -1 if no token (optional) + int8_t depth; // depth of object in the tree. 0 is root (-1 is invalid) + int8_t valuetype; // see valueType enum + int8_t precision; // decimal precision for reporting (JSON) + float value; // numeric value + char_t group[GROUP_LEN+1]; // group prefix or 0 if not in a group + char_t token[TOKEN_LEN+1]; // full mnemonic token for lookup + char_t (*stringp)[]; // pointer to array of characters from shared character array +} nvObj_t; // OK, so it's not REALLY an object typedef uint8_t (*fptrCmd)(nvObj_t *nv);// required for cfg table access -typedef void (*fptrPrint)(nvObj_t *nv); // required for PROGMEM access +typedef void (*fptrPrint)(nvObj_t *nv); // required for PROGMEM access typedef struct nvList { - uint16_t magic_start; - nvObj_t list[NV_LIST_LEN]; // list of nv objects, including space for a JSON header element - uint16_t magic_end; + uint16_t magic_start; + nvObj_t list[NV_LIST_LEN]; // list of nv objects, including space for a JSON header element + uint16_t magic_end; } nvList_t; typedef struct cfgItem { - char_t group[GROUP_LEN+1]; // group prefix (with NUL termination) - char_t token[TOKEN_LEN+1]; // token - stripped of group prefix (w/NUL termination) - uint8_t flags; // operations flags - see defines below - int8_t precision; // decimal precision for display (JSON) - fptrPrint print; // print binding: aka void (*print)(nvObj_t *nv); - fptrCmd get; // GET binding aka uint8_t (*get)(nvObj_t *nv) - fptrCmd set; // SET binding aka uint8_t (*set)(nvObj_t *nv) - float *target; // target for writing config value - float def_value; // default value for config item + char_t group[GROUP_LEN+1]; // group prefix (with 0 termination) + char_t token[TOKEN_LEN+1]; // token - stripped of group prefix (w/0 termination) + uint8_t flags; // operations flags - see defines below + int8_t precision; // decimal precision for display (JSON) + fptrPrint print; // print binding: aka void (*print)(nvObj_t *nv); + fptrCmd get; // GET binding aka uint8_t (*get)(nvObj_t *nv) + fptrCmd set; // SET binding aka uint8_t (*set)(nvObj_t *nv) + float *target; // target for writing config value + float def_value; // default value for config item } cfgItem_t; /**** static allocation and definitions ****/ @@ -306,49 +306,49 @@ extern const cfgItem_t cfgArray[]; /**** Prototypes for generic config functions - see individual modules for application-specific functions ****/ -void config_init(void); -stat_t set_defaults(nvObj_t *nv); // reset config to default values -void config_init_assertions(void); -stat_t config_test_assertions(void); +void config_init(); +stat_t set_defaults(nvObj_t *nv); // reset config to default values +void config_init_assertions(); +stat_t config_test_assertions(); // main entry points for core access functions -stat_t nv_get(nvObj_t *nv); // main entry point for get value -stat_t nv_set(nvObj_t *nv); // main entry point for set value -void nv_print(nvObj_t *nv); // main entry point for print value -stat_t nv_persist(nvObj_t *nv); // main entry point for persistence +stat_t nv_get(nvObj_t *nv); // main entry point for get value +stat_t nv_set(nvObj_t *nv); // main entry point for set value +void nv_print(nvObj_t *nv); // main entry point for print value +stat_t nv_persist(nvObj_t *nv); // main entry point for persistence // helpers uint8_t nv_get_type(nvObj_t *nv); index_t nv_get_index(const char_t *group, const char_t *token); -index_t nv_index_max(void); // (see config_app.c) -uint8_t nv_index_is_single(index_t index); // (see config_app.c) -uint8_t nv_index_is_group(index_t index); // (see config_app.c) -uint8_t nv_index_lt_groups(index_t index); // (see config_app.c) +index_t nv_index_max(); // (see config_app.c) +uint8_t nv_index_is_single(index_t index); // (see config_app.c) +uint8_t nv_index_is_group(index_t index); // (see config_app.c) +uint8_t nv_index_lt_groups(index_t index); // (see config_app.c) uint8_t nv_group_is_prefixed(char_t *group); // generic internal functions and accessors -stat_t set_nul(nvObj_t *nv); // set nothing (no operation) -stat_t set_ui8(nvObj_t *nv); // set uint8_t value -stat_t set_01(nvObj_t *nv); // set a 0 or 1 value with validation -stat_t set_012(nvObj_t *nv); // set a 0, 1 or 2 value with validation -stat_t set_0123(nvObj_t *nv); // set a 0, 1, 2 or 3 value with validation -stat_t set_int(nvObj_t *nv); // set uint32_t integer value -stat_t set_data(nvObj_t *nv); // set uint32_t integer value blind cast -stat_t set_flt(nvObj_t *nv); // set floating point value - -stat_t get_nul(nvObj_t *nv); // get null value type -stat_t get_ui8(nvObj_t *nv); // get uint8_t value -stat_t get_int(nvObj_t *nv); // get uint32_t integer value -stat_t get_data(nvObj_t *nv); // get uint32_t integer value blind cast -stat_t get_flt(nvObj_t *nv); // get floating point value - -stat_t set_grp(nvObj_t *nv); // set data for a group -stat_t get_grp(nvObj_t *nv); // get data for a group +stat_t set_nul(nvObj_t *nv); // set nothing (no operation) +stat_t set_ui8(nvObj_t *nv); // set uint8_t value +stat_t set_01(nvObj_t *nv); // set a 0 or 1 value with validation +stat_t set_012(nvObj_t *nv); // set a 0, 1 or 2 value with validation +stat_t set_0123(nvObj_t *nv); // set a 0, 1, 2 or 3 value with validation +stat_t set_int(nvObj_t *nv); // set uint32_t integer value +stat_t set_data(nvObj_t *nv); // set uint32_t integer value blind cast +stat_t set_flt(nvObj_t *nv); // set floating point value + +stat_t get_nul(nvObj_t *nv); // get null value type +stat_t get_ui8(nvObj_t *nv); // get uint8_t value +stat_t get_int(nvObj_t *nv); // get uint32_t integer value +stat_t get_data(nvObj_t *nv); // get uint32_t integer value blind cast +stat_t get_flt(nvObj_t *nv); // get floating point value + +stat_t set_grp(nvObj_t *nv); // set data for a group +stat_t get_grp(nvObj_t *nv); // get data for a group // nvObj and list functions void nv_get_nvObj(nvObj_t *nv); nvObj_t *nv_reset_nv(nvObj_t *nv); -nvObj_t *nv_reset_nv_list(void); +nvObj_t *nv_reset_nv_list(); stat_t nv_copy_string(nvObj_t *nv, const char_t *src); nvObj_t *nv_add_object(const char_t *token); nvObj_t *nv_add_integer(const char_t *token, const uint32_t value); @@ -358,8 +358,8 @@ nvObj_t *nv_add_conditional_message(const char_t *string); void nv_print_list(stat_t status, uint8_t text_flags, uint8_t json_flags); // application specific helpers and functions (config_app.c) -stat_t set_flu(nvObj_t *nv); // set floating point number with G20/G21 units conversion -void preprocess_float(nvObj_t *nv); // pre-process float values for units and illegal values +stat_t set_flu(nvObj_t *nv); // set floating point number with G20/G21 units conversion +void preprocess_float(nvObj_t *nv); // pre-process float values for units and illegal values // diagnostics void nv_dump_nv(nvObj_t *nv); diff --git a/src/config_app.c b/src/config_app.c index 3a84c50..c2273e0 100755 --- a/src/config_app.c +++ b/src/config_app.c @@ -17,16 +17,16 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* This file contains application specific data for the config system: - * - application-specific functions and function prototypes - * - application-specific message and print format strings - * - application-specific config array - * - any other application-specific data or functions + * - application-specific functions and function prototypes + * - application-specific message and print format strings + * - application-specific config array + * - any other application-specific data or functions * * See config_app.h for a detailed description of config objects and the config table */ -#include "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "controller.h" #include "canonical_machine.h" #include "gcode_parser.h" @@ -43,656 +43,622 @@ #include "util.h" #include "help.h" #include "network.h" -#include "xio.h" +#include "xio/xio.h" -/*** structures ***/ +cfgParameters_t cfg; // application specific configuration parameters -cfgParameters_t cfg; // application specific configuration parameters - -/*********************************************************************************** - **** application-specific internal functions ************************************** - ***********************************************************************************/ // See config.cpp/.h for generic variables and functions that are not specific to // TinyG or the motion control application domain // helpers (most helpers are defined immediately above their usage so they don't need prototypes here) -static stat_t _do_motors(nvObj_t *nv); // print parameters for all motor groups -static stat_t _do_axes(nvObj_t *nv); // print parameters for all axis groups -static stat_t _do_offsets(nvObj_t *nv); // print offset parameters for G54-G59,G92, G28, G30 -static stat_t _do_all(nvObj_t *nv); // print all parameters +static stat_t _do_motors(nvObj_t *nv); // print parameters for all motor groups +static stat_t _do_axes(nvObj_t *nv); // print parameters for all axis groups +static stat_t _do_offsets(nvObj_t *nv); // print offset parameters for G54-G59,G92, G28, G30 +static stat_t _do_all(nvObj_t *nv); // print all parameters // communications settings and functions -static stat_t set_ec(nvObj_t *nv); // expand CRLF on TX output -static stat_t set_ee(nvObj_t *nv); // enable character echo -static stat_t set_ex(nvObj_t *nv); // enable XON/XOFF and RTS/CTS flow control -static stat_t set_baud(nvObj_t *nv); // set USB baud rate -static stat_t get_rx(nvObj_t *nv); // get bytes in RX buffer -//static stat_t run_sx(nvObj_t *nv); // send XOFF, XON +static stat_t set_ec(nvObj_t *nv); // expand CRLF on TX output +static stat_t set_ee(nvObj_t *nv); // enable character echo +static stat_t set_ex(nvObj_t *nv); // enable XON/XOFF and RTS/CTS flow control +static stat_t set_baud(nvObj_t *nv); // set USB baud rate +static stat_t get_rx(nvObj_t *nv); // get bytes in RX buffer + /*********************************************************************************** **** CONFIG TABLE **************************************************************** *********************************************************************************** * - * NOTES AND CAVEATS + * NOTES AND CAVEATS * - * - Token matching occurs from the most specific to the least specific. This means - * that if shorter tokens overlap longer ones the longer one must precede the - * shorter one. E.g. "gco" needs to come before "gc" + * - Token matching occurs from the most specific to the least specific. This means + * that if shorter tokens overlap longer ones the longer one must precede the + * shorter one. E.g. "gco" needs to come before "gc" * - * - Mark group strings for entries that have no group as nul --> "". - * This is important for group expansion. + * - Mark group strings for entries that have no group as nul --> "". + * This is important for group expansion. * - * - Groups do not have groups. Neither do uber-groups, e.g. - * 'x' is --> { "", "x", and 'm' is --> { "", "m", + * - Groups do not have groups. Neither do uber-groups, e.g. + * 'x' is --> { "", "x", and 'm' is --> { "", "m", * - * - Be careful not to define groups longer than GROUP_LEN (3) and tokens longer - * than TOKEN_LEN (5). (See config.h for lengths). The combined group + token - * cannot exceed TOKEN_LEN. String functions working on the table assume these - * rules are followed and do not check lengths or perform other validation. + * - Be careful not to define groups longer than GROUP_LEN (3) and tokens longer + * than TOKEN_LEN (5). (See config.h for lengths). The combined group + token + * cannot exceed TOKEN_LEN. String functions working on the table assume these + * rules are followed and do not check lengths or perform other validation. * - * NOTE: If the count of lines in cfgArray exceeds 255 you need to change index_t - * uint16_t in the config.h file. + * NOTE: If the count of lines in cfgArray exceeds 255 you need to change index_t + * uint16_t in the config.h file. */ - const cfgItem_t cfgArray[] PROGMEM = { - // group token flags p, print_func, get_func, set_func, target for get/set, default value - { "sys", "fb", _fipn,2, hw_print_fb, get_flt, set_nul, (float *)&cs.fw_build, TINYG_FIRMWARE_BUILD }, // MUST BE FIRST! - { "sys", "fv", _fipn,3, hw_print_fv, get_flt, set_nul, (float *)&cs.fw_version, TINYG_FIRMWARE_VERSION }, - { "sys", "hp", _fipn,0, hw_print_hp, get_flt, set_flt, (float *)&cs.hw_platform,TINYG_HARDWARE_PLATFORM }, - { "sys", "hv", _fipn,0, hw_print_hv, get_flt, hw_set_hv,(float *)&cs.hw_version, TINYG_HARDWARE_VERSION }, - { "sys", "id", _fn, 0, hw_print_id, hw_get_id, set_nul, (float *)&cs.null, 0 }, // device ID (ASCII signature) - - // dynamic model attributes for reporting purposes (up front for speed) - { "", "n", _fi, 0, cm_print_line, cm_get_mline,set_int,(float *)&cm.gm.linenum,0 }, // Model line number - { "", "line",_fi, 0, cm_print_line, cm_get_line, set_int,(float *)&cm.gm.linenum,0 }, // Active line number - model or runtime line number - { "", "vel", _f0, 2, cm_print_vel, cm_get_vel, set_nul,(float *)&cs.null, 0 }, // current velocity - { "", "feed",_f0, 2, cm_print_feed, cm_get_feed, set_nul,(float *)&cs.null, 0 }, // feed rate - { "", "stat",_f0, 0, cm_print_stat, cm_get_stat, set_nul,(float *)&cs.null, 0 }, // combined machine state - { "", "macs",_f0, 0, cm_print_macs, cm_get_macs, set_nul,(float *)&cs.null, 0 }, // raw machine state - { "", "cycs",_f0, 0, cm_print_cycs, cm_get_cycs, set_nul,(float *)&cs.null, 0 }, // cycle state - { "", "mots",_f0, 0, cm_print_mots, cm_get_mots, set_nul,(float *)&cs.null, 0 }, // motion state - { "", "hold",_f0, 0, cm_print_hold, cm_get_hold, set_nul,(float *)&cs.null, 0 }, // feedhold state - { "", "unit",_f0, 0, cm_print_unit, cm_get_unit, set_nul,(float *)&cs.null, 0 }, // units mode - { "", "coor",_f0, 0, cm_print_coor, cm_get_coor, set_nul,(float *)&cs.null, 0 }, // coordinate system - { "", "momo",_f0, 0, cm_print_momo, cm_get_momo, set_nul,(float *)&cs.null, 0 }, // motion mode - { "", "plan",_f0, 0, cm_print_plan, cm_get_plan, set_nul,(float *)&cs.null, 0 }, // plane select - { "", "path",_f0, 0, cm_print_path, cm_get_path, set_nul,(float *)&cs.null, 0 }, // path control mode - { "", "dist",_f0, 0, cm_print_dist, cm_get_dist, set_nul,(float *)&cs.null, 0 }, // distance mode - { "", "frmo",_f0, 0, cm_print_frmo, cm_get_frmo, set_nul,(float *)&cs.null, 0 }, // feed rate mode - { "", "tool",_f0, 0, cm_print_tool, cm_get_toolv,set_nul,(float *)&cs.null, 0 }, // active tool -// { "", "tick",_f0, 0, tx_print_int, get_int, set_int,(float *)&rtc.sys_ticks, 0 }, // tick count - - { "mpo","mpox",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0 }, // X machine position - { "mpo","mpoy",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0 }, // Y machine position - { "mpo","mpoz",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0 }, // Z machine position - { "mpo","mpoa",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0 }, // A machine position - { "mpo","mpob",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0 }, // B machine position - { "mpo","mpoc",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0 }, // C machine position - - { "pos","posx",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0 }, // X work position - { "pos","posy",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0 }, // Y work position - { "pos","posz",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0 }, // Z work position - { "pos","posa",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0 }, // A work position - { "pos","posb",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0 }, // B work position - { "pos","posc",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0 }, // C work position - - { "ofs","ofsx",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0 }, // X work offset - { "ofs","ofsy",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0 }, // Y work offset - { "ofs","ofsz",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0 }, // Z work offset - { "ofs","ofsa",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0 }, // A work offset - { "ofs","ofsb",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0 }, // B work offset - { "ofs","ofsc",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0 }, // C work offset - - { "hom","home",_f0, 0, cm_print_home, cm_get_home, cm_run_home,(float *)&cs.null, 0 }, // homing state, invoke homing cycle - { "hom","homx",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_X], false }, // X homed - Homing status group - { "hom","homy",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_Y], false }, // Y homed - { "hom","homz",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_Z], false }, // Z homed - { "hom","homa",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_A], false }, // A homed - { "hom","homb",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_B], false }, // B homed - { "hom","homc",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_C], false }, // C homed - - { "prb","prbe",_f0, 0, tx_print_nul, get_ui8, set_nul,(float *)&cm.probe_state, 0 }, // probing state - { "prb","prbx",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_X], 0 }, - { "prb","prby",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_Y], 0 }, - { "prb","prbz",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_Z], 0 }, - { "prb","prba",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_A], 0 }, - { "prb","prbb",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_B], 0 }, - { "prb","prbc",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_C], 0 }, - - { "jog","jogx",_f0, 0, tx_print_nul, get_nul, cm_run_jogx, (float *)&cm.jogging_dest, 0}, - { "jog","jogy",_f0, 0, tx_print_nul, get_nul, cm_run_jogy, (float *)&cm.jogging_dest, 0}, - { "jog","jogz",_f0, 0, tx_print_nul, get_nul, cm_run_jogz, (float *)&cm.jogging_dest, 0}, - { "jog","joga",_f0, 0, tx_print_nul, get_nul, cm_run_joga, (float *)&cm.jogging_dest, 0}, -// { "jog","jogb",_f0, 0, tx_print_nul, get_nul, cm_run_jogb, (float *)&cm.jogging_dest, 0}, -// { "jog","jogc",_f0, 0, tx_print_nul, get_nul, cm_run_jogc, (float *)&cm.jogging_dest, 0}, - - { "pwr","pwr1",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, // motor power enable readouts - { "pwr","pwr2",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, - { "pwr","pwr3",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, - { "pwr","pwr4",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, + // group token flags p, print_func, get_func, set_func, target for get/set, default value + {"sys", "fb", _fipn,2, hw_print_fb, get_flt, set_nul, (float *)&cs.fw_build, TINYG_FIRMWARE_BUILD}, // MUST BE FIRST! + {"sys", "fv", _fipn,3, hw_print_fv, get_flt, set_nul, (float *)&cs.fw_version, TINYG_FIRMWARE_VERSION}, + {"sys", "hp", _fipn,0, hw_print_hp, get_flt, set_flt, (float *)&cs.hw_platform,TINYG_HARDWARE_PLATFORM}, + {"sys", "hv", _fipn,0, hw_print_hv, get_flt, hw_set_hv,(float *)&cs.hw_version, TINYG_HARDWARE_VERSION}, + {"sys", "id", _fn, 0, hw_print_id, hw_get_id, set_nul, (float *)&cs.null, 0}, // device ID (ASCII signature) + + // dynamic model attributes for reporting purposes (up front for speed) + {"", "n", _fi, 0, cm_print_line, cm_get_mline,set_int,(float *)&cm.gm.linenum,0}, // Model line number + {"", "line",_fi, 0, cm_print_line, cm_get_line, set_int,(float *)&cm.gm.linenum,0}, // Active line number - model or runtime line number + {"", "vel", _f0, 2, cm_print_vel, cm_get_vel, set_nul,(float *)&cs.null, 0}, // current velocity + {"", "feed",_f0, 2, cm_print_feed, cm_get_feed, set_nul,(float *)&cs.null, 0}, // feed rate + {"", "stat",_f0, 0, cm_print_stat, cm_get_stat, set_nul,(float *)&cs.null, 0}, // combined machine state + {"", "macs",_f0, 0, cm_print_macs, cm_get_macs, set_nul,(float *)&cs.null, 0}, // raw machine state + {"", "cycs",_f0, 0, cm_print_cycs, cm_get_cycs, set_nul,(float *)&cs.null, 0}, // cycle state + {"", "mots",_f0, 0, cm_print_mots, cm_get_mots, set_nul,(float *)&cs.null, 0}, // motion state + {"", "hold",_f0, 0, cm_print_hold, cm_get_hold, set_nul,(float *)&cs.null, 0}, // feedhold state + {"", "unit",_f0, 0, cm_print_unit, cm_get_unit, set_nul,(float *)&cs.null, 0}, // units mode + {"", "coor",_f0, 0, cm_print_coor, cm_get_coor, set_nul,(float *)&cs.null, 0}, // coordinate system + {"", "momo",_f0, 0, cm_print_momo, cm_get_momo, set_nul,(float *)&cs.null, 0}, // motion mode + {"", "plan",_f0, 0, cm_print_plan, cm_get_plan, set_nul,(float *)&cs.null, 0}, // plane select + {"", "path",_f0, 0, cm_print_path, cm_get_path, set_nul,(float *)&cs.null, 0}, // path control mode + {"", "dist",_f0, 0, cm_print_dist, cm_get_dist, set_nul,(float *)&cs.null, 0}, // distance mode + {"", "frmo",_f0, 0, cm_print_frmo, cm_get_frmo, set_nul,(float *)&cs.null, 0}, // feed rate mode + {"", "tool",_f0, 0, cm_print_tool, cm_get_toolv,set_nul,(float *)&cs.null, 0}, // active tool + + {"mpo","mpox",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // X machine position + {"mpo","mpoy",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // Y machine position + {"mpo","mpoz",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // Z machine position + {"mpo","mpoa",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // A machine position + {"mpo","mpob",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // B machine position + {"mpo","mpoc",_f0, 3, cm_print_mpo, cm_get_mpo, set_nul,(float *)&cs.null, 0}, // C machine position + + {"pos","posx",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // X work position + {"pos","posy",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // Y work position + {"pos","posz",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // Z work position + {"pos","posa",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // A work position + {"pos","posb",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // B work position + {"pos","posc",_f0, 3, cm_print_pos, cm_get_pos, set_nul,(float *)&cs.null, 0}, // C work position + + {"ofs","ofsx",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // X work offset + {"ofs","ofsy",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // Y work offset + {"ofs","ofsz",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // Z work offset + {"ofs","ofsa",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // A work offset + {"ofs","ofsb",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // B work offset + {"ofs","ofsc",_f0, 3, cm_print_ofs, cm_get_ofs, set_nul,(float *)&cs.null, 0}, // C work offset + + {"hom","home",_f0, 0, cm_print_home, cm_get_home, cm_run_home,(float *)&cs.null, 0}, // homing state, invoke homing cycle + {"hom","homx",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_X], false}, // X homed - Homing status group + {"hom","homy",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_Y], false}, // Y homed + {"hom","homz",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_Z], false}, // Z homed + {"hom","homa",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_A], false}, // A homed + {"hom","homb",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_B], false}, // B homed + {"hom","homc",_f0, 0, cm_print_pos, get_ui8, set_nul,(float *)&cm.homed[AXIS_C], false}, // C homed + + {"prb","prbe",_f0, 0, tx_print_nul, get_ui8, set_nul,(float *)&cm.probe_state, 0}, // probing state + {"prb","prbx",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_X], 0}, + {"prb","prby",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_Y], 0}, + {"prb","prbz",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_Z], 0}, + {"prb","prba",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_A], 0}, + {"prb","prbb",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_B], 0}, + {"prb","prbc",_f0, 3, tx_print_nul, get_flt, set_nul,(float *)&cm.probe_results[AXIS_C], 0}, + + {"jog","jogx",_f0, 0, tx_print_nul, get_nul, cm_run_jogx, (float *)&cm.jogging_dest, 0}, + {"jog","jogy",_f0, 0, tx_print_nul, get_nul, cm_run_jogy, (float *)&cm.jogging_dest, 0}, + {"jog","jogz",_f0, 0, tx_print_nul, get_nul, cm_run_jogz, (float *)&cm.jogging_dest, 0}, + {"jog","joga",_f0, 0, tx_print_nul, get_nul, cm_run_joga, (float *)&cm.jogging_dest, 0}, + + {"pwr","pwr1",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, // motor power enable readouts + {"pwr","pwr2",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, + {"pwr","pwr3",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, + {"pwr","pwr4",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, #if (MOTORS >= 5) - { "pwr","pwr5",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, + {"pwr","pwr5",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, #endif #if (MOTORS >= 6) - { "pwr","pwr6",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, + {"pwr","pwr6",_f0, 0, st_print_pwr, st_get_pwr, set_nul, (float *)&cs.null, 0}, #endif - // Reports, tests, help, and messages - { "", "sr", _f0, 0, sr_print_sr, sr_get, sr_set, (float *)&cs.null, 0 }, // status report object - { "", "qr", _f0, 0, qr_print_qr, qr_get, set_nul, (float *)&cs.null, 0 }, // queue report - planner buffers available - { "", "qi", _f0, 0, qr_print_qi, qi_get, set_nul, (float *)&cs.null, 0 }, // queue report - buffers added to queue - { "", "qo", _f0, 0, qr_print_qo, qo_get, set_nul, (float *)&cs.null, 0 }, // queue report - buffers removed from queue - { "", "er", _f0, 0, tx_print_nul, rpt_er, set_nul, (float *)&cs.null, 0 }, // invoke bogus exception report for testing - { "", "qf", _f0, 0, tx_print_nul, get_nul, cm_run_qf,(float *)&cs.null, 0 }, // queue flush - { "", "rx", _f0, 0, tx_print_int, get_rx, set_nul, (float *)&cs.null, 0 }, // space in RX buffer - { "", "msg", _f0, 0, tx_print_str, get_nul, set_nul, (float *)&cs.null, 0 }, // string for generic messages -// { "", "clc", _f0, 0, tx_print_nul, st_clc, st_clc, (float *)&cs.null, 0 }, // clear diagnostic step counters - { "", "clear",_f0,0, tx_print_nul, cm_clear,cm_clear, (float *)&cs.null, 0 }, // GET a clear to clear soft alarm -// { "", "sx", _f0, 0, tx_print_nul, run_sx, run_sx , (float *)&cs.null, 0 }, // send XOFF, XON test - - { "", "test",_f0, 0, tx_print_nul, help_test, run_test, (float *)&cs.null,0 }, // run tests, print test help screen - { "", "defa",_f0, 0, tx_print_nul, help_defa, set_defaults,(float *)&cs.null,0 }, // set/print defaults / help screen - { "", "boot",_f0, 0, tx_print_nul, help_boot_loader,hw_run_boot, (float *)&cs.null,0 }, + // Reports, tests, help, and messages + {"", "sr", _f0, 0, sr_print_sr, sr_get, sr_set, (float *)&cs.null, 0}, // status report object + {"", "qr", _f0, 0, qr_print_qr, qr_get, set_nul, (float *)&cs.null, 0}, // queue report - planner buffers available + {"", "qi", _f0, 0, qr_print_qi, qi_get, set_nul, (float *)&cs.null, 0}, // queue report - buffers added to queue + {"", "qo", _f0, 0, qr_print_qo, qo_get, set_nul, (float *)&cs.null, 0}, // queue report - buffers removed from queue + {"", "er", _f0, 0, tx_print_nul, rpt_er, set_nul, (float *)&cs.null, 0}, // invoke bogus exception report for testing + {"", "qf", _f0, 0, tx_print_nul, get_nul, cm_run_qf,(float *)&cs.null, 0}, // queue flush + {"", "rx", _f0, 0, tx_print_int, get_rx, set_nul, (float *)&cs.null, 0}, // space in RX buffer + {"", "msg", _f0, 0, tx_print_str, get_nul, set_nul, (float *)&cs.null, 0}, // string for generic messages + {"", "clear",_f0,0, tx_print_nul, cm_clear,cm_clear, (float *)&cs.null, 0}, // GET a clear to clear soft alarm + + {"", "test",_f0, 0, tx_print_nul, help_test, run_test, (float *)&cs.null,0}, // run tests, print test help screen + {"", "defa",_f0, 0, tx_print_nul, help_defa, set_defaults,(float *)&cs.null,0}, // set/print defaults / help screen + {"", "boot",_f0, 0, tx_print_nul, help_boot_loader,hw_run_boot, (float *)&cs.null,0}, #ifdef __HELP_SCREENS - { "", "help",_f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0 }, // prints config help screen - { "", "h", _f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0 }, // alias for "help" + {"", "help",_f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0}, // prints config help screen + {"", "h", _f0, 0, tx_print_nul, help_config, set_nul, (float *)&cs.null,0}, // alias for "help" #endif - // Motor parameters - { "1","1ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_1].motor_map, M1_MOTOR_MAP }, - { "1","1sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_1].step_angle, M1_STEP_ANGLE }, - { "1","1tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_1].travel_rev, M1_TRAVEL_PER_REV }, - { "1","1mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_1].microsteps, M1_MICROSTEPS }, - { "1","1po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_1].polarity, M1_POLARITY }, - { "1","1pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_1].power_mode, M1_POWER_MODE }, + // Motor parameters + {"1","1ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_1].motor_map, M1_MOTOR_MAP}, + {"1","1sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_1].step_angle, M1_STEP_ANGLE}, + {"1","1tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_1].travel_rev, M1_TRAVEL_PER_REV}, + {"1","1mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_1].microsteps, M1_MICROSTEPS}, + {"1","1po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_1].polarity, M1_POLARITY}, + {"1","1pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_1].power_mode, M1_POWER_MODE}, #if (MOTORS >= 2) - { "2","2ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_2].motor_map, M2_MOTOR_MAP }, - { "2","2sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_2].step_angle, M2_STEP_ANGLE }, - { "2","2tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_2].travel_rev, M2_TRAVEL_PER_REV }, - { "2","2mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_2].microsteps, M2_MICROSTEPS }, - { "2","2po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_2].polarity, M2_POLARITY }, - { "2","2pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_2].power_mode, M2_POWER_MODE }, + {"2","2ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_2].motor_map, M2_MOTOR_MAP}, + {"2","2sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_2].step_angle, M2_STEP_ANGLE}, + {"2","2tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_2].travel_rev, M2_TRAVEL_PER_REV}, + {"2","2mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_2].microsteps, M2_MICROSTEPS}, + {"2","2po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_2].polarity, M2_POLARITY}, + {"2","2pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_2].power_mode, M2_POWER_MODE}, #endif #if (MOTORS >= 3) - { "3","3ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_3].motor_map, M3_MOTOR_MAP }, - { "3","3sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_3].step_angle, M3_STEP_ANGLE }, - { "3","3tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_3].travel_rev, M3_TRAVEL_PER_REV }, - { "3","3mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_3].microsteps, M3_MICROSTEPS }, - { "3","3po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_3].polarity, M3_POLARITY }, - { "3","3pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_3].power_mode, M3_POWER_MODE }, + {"3","3ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_3].motor_map, M3_MOTOR_MAP}, + {"3","3sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_3].step_angle, M3_STEP_ANGLE}, + {"3","3tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_3].travel_rev, M3_TRAVEL_PER_REV}, + {"3","3mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_3].microsteps, M3_MICROSTEPS}, + {"3","3po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_3].polarity, M3_POLARITY}, + {"3","3pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_3].power_mode, M3_POWER_MODE}, #endif #if (MOTORS >= 4) - { "4","4ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_4].motor_map, M4_MOTOR_MAP }, - { "4","4sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_4].step_angle, M4_STEP_ANGLE }, - { "4","4tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_4].travel_rev, M4_TRAVEL_PER_REV }, - { "4","4mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_4].microsteps, M4_MICROSTEPS }, - { "4","4po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_4].polarity, M4_POLARITY }, - { "4","4pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_4].power_mode, M4_POWER_MODE }, + {"4","4ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_4].motor_map, M4_MOTOR_MAP}, + {"4","4sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_4].step_angle, M4_STEP_ANGLE}, + {"4","4tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_4].travel_rev, M4_TRAVEL_PER_REV}, + {"4","4mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_4].microsteps, M4_MICROSTEPS}, + {"4","4po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_4].polarity, M4_POLARITY}, + {"4","4pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_4].power_mode, M4_POWER_MODE}, #endif #if (MOTORS >= 5) - { "5","5ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_5].motor_map, M5_MOTOR_MAP }, - { "5","5sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_5].step_angle, M5_STEP_ANGLE }, - { "5","5tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_5].travel_rev, M5_TRAVEL_PER_REV }, - { "5","5mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_5].microsteps, M5_MICROSTEPS }, - { "5","5po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_5].polarity, M5_POLARITY }, - { "5","5pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_5].power_mode, M5_POWER_MODE }, + {"5","5ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_5].motor_map, M5_MOTOR_MAP}, + {"5","5sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_5].step_angle, M5_STEP_ANGLE}, + {"5","5tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_5].travel_rev, M5_TRAVEL_PER_REV}, + {"5","5mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_5].microsteps, M5_MICROSTEPS}, + {"5","5po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_5].polarity, M5_POLARITY}, + {"5","5pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_5].power_mode, M5_POWER_MODE}, #endif #if (MOTORS >= 6) - { "6","6ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_6].motor_map, M6_MOTOR_MAP }, - { "6","6sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_6].step_angle, M6_STEP_ANGLE }, - { "6","6tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_6].travel_rev, M6_TRAVEL_PER_REV }, - { "6","6mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_6].microsteps, M6_MICROSTEPS }, - { "6","6po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_6].polarity, M6_POLARITY }, - { "6","6pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_6].power_mode, M6_POWER_MODE }, + {"6","6ma",_fip, 0, st_print_ma, get_ui8, set_ui8, (float *)&st_cfg.mot[MOTOR_6].motor_map, M6_MOTOR_MAP}, + {"6","6sa",_fip, 3, st_print_sa, get_flt, st_set_sa, (float *)&st_cfg.mot[MOTOR_6].step_angle, M6_STEP_ANGLE}, + {"6","6tr",_fipc,4, st_print_tr, get_flt, st_set_tr, (float *)&st_cfg.mot[MOTOR_6].travel_rev, M6_TRAVEL_PER_REV}, + {"6","6mi",_fip, 0, st_print_mi, get_ui8, st_set_mi, (float *)&st_cfg.mot[MOTOR_6].microsteps, M6_MICROSTEPS}, + {"6","6po",_fip, 0, st_print_po, get_ui8, set_01, (float *)&st_cfg.mot[MOTOR_6].polarity, M6_POLARITY}, + {"6","6pm",_fip, 0, st_print_pm, get_ui8, st_set_pm, (float *)&st_cfg.mot[MOTOR_6].power_mode, M6_POWER_MODE}, #endif - // Axis parameters - { "x","xam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_X].axis_mode, X_AXIS_MODE }, - { "x","xvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_X].velocity_max, X_VELOCITY_MAX }, - { "x","xfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_X].feedrate_max, X_FEEDRATE_MAX }, - { "x","xtn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_min, X_TRAVEL_MIN }, - { "x","xtm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_max, X_TRAVEL_MAX }, - { "x","xjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_X].jerk_max, X_JERK_MAX }, - { "x","xjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_X].jerk_homing, X_JERK_HOMING }, - { "x","xjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_X].junction_dev, X_JUNCTION_DEVIATION }, - { "x","xsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[0], X_SWITCH_MODE_MIN }, - { "x","xsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[1], X_SWITCH_MODE_MAX }, -// { "x","xsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_X][SW_MIN].mode, X_SWITCH_MODE_MIN }, // new style -// { "x","xsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_X][SW_MAX].mode, X_SWITCH_MODE_MAX }, // new style - { "x","xsv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_X].search_velocity,X_SEARCH_VELOCITY }, - { "x","xlv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_X].latch_velocity, X_LATCH_VELOCITY }, - { "x","xlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_X].latch_backoff, X_LATCH_BACKOFF }, - { "x","xzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_X].zero_backoff, X_ZERO_BACKOFF }, - - { "y","yam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Y].axis_mode, Y_AXIS_MODE }, - { "y","yvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_Y].velocity_max, Y_VELOCITY_MAX }, - { "y","yfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_Y].feedrate_max, Y_FEEDRATE_MAX }, - { "y","ytn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_min, Y_TRAVEL_MIN }, - { "y","ytm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_max, Y_TRAVEL_MAX }, - { "y","yjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_Y].jerk_max, Y_JERK_MAX }, - { "y","yjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_Y].jerk_homing, Y_JERK_HOMING }, - { "y","yjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_Y].junction_dev, Y_JUNCTION_DEVIATION }, - { "y","ysn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[2], Y_SWITCH_MODE_MIN }, - { "y","ysx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[3], Y_SWITCH_MODE_MAX }, -// { "y","ysn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_Y][SW_MIN].mode, Y_SWITCH_MODE_MIN }, // new style -// { "y","ysx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_Y][SW_MAX].mode, Y_SWITCH_MODE_MAX }, // new style - { "y","ysv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_Y].search_velocity,Y_SEARCH_VELOCITY }, - { "y","ylv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_Y].latch_velocity, Y_LATCH_VELOCITY }, - { "y","ylb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].latch_backoff, Y_LATCH_BACKOFF }, - { "y","yzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].zero_backoff, Y_ZERO_BACKOFF }, - - { "z","zam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Z].axis_mode, Z_AXIS_MODE }, - { "z","zvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_Z].velocity_max, Z_VELOCITY_MAX }, - { "z","zfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_Z].feedrate_max, Z_FEEDRATE_MAX }, - { "z","ztn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_min, Z_TRAVEL_MIN }, - { "z","ztm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_max, Z_TRAVEL_MAX }, - { "z","zjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_Z].jerk_max, Z_JERK_MAX }, - { "z","zjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_Z].jerk_homing, Z_JERK_HOMING }, - { "z","zjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_Z].junction_dev, Z_JUNCTION_DEVIATION }, - { "z","zsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[4], Z_SWITCH_MODE_MIN }, - { "z","zsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[5], Z_SWITCH_MODE_MAX }, -// { "z","zsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_Z][SW_MIN].mode, Z_SWITCH_MODE_MIN }, // new style -// { "z","zsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_Z][SW_MAX].mode, Z_SWITCH_MODE_MAX }, // new style - { "z","zsv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_Z].search_velocity,Z_SEARCH_VELOCITY }, - { "z","zlv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_Z].latch_velocity, Z_LATCH_VELOCITY }, - { "z","zlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].latch_backoff, Z_LATCH_BACKOFF }, - { "z","zzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].zero_backoff, Z_ZERO_BACKOFF }, - - { "a","aam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_A].axis_mode, A_AXIS_MODE }, - { "a","avm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_A].velocity_max, A_VELOCITY_MAX }, - { "a","afr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_A].feedrate_max, A_FEEDRATE_MAX }, - { "a","atn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_A].travel_min, A_TRAVEL_MIN }, - { "a","atm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_A].travel_max, A_TRAVEL_MAX }, - { "a","ajm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_A].jerk_max, A_JERK_MAX }, - { "a","ajh",_fip, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_A].jerk_homing, A_JERK_HOMING }, - { "a","ajd",_fip, 4, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_A].junction_dev, A_JUNCTION_DEVIATION }, - { "a","ara",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_A].radius, A_RADIUS}, - { "a","asn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[6], A_SWITCH_MODE_MIN }, - { "a","asx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[7], A_SWITCH_MODE_MAX }, -// { "a","asn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_A][SW_MIN].mode, A_SWITCH_MODE_MIN }, // new style -// { "a","asx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.s[AXIS_A][SW_MAX].mode, A_SWITCH_MODE_MAX }, // new style - { "a","asv",_fip, 0, cm_print_sv, get_flt, set_flt, (float *)&cm.a[AXIS_A].search_velocity,A_SEARCH_VELOCITY }, - { "a","alv",_fip, 0, cm_print_lv, get_flt, set_flt, (float *)&cm.a[AXIS_A].latch_velocity, A_LATCH_VELOCITY }, - { "a","alb",_fip, 3, cm_print_lb, get_flt, set_flt, (float *)&cm.a[AXIS_A].latch_backoff, A_LATCH_BACKOFF }, - { "a","azb",_fip, 3, cm_print_zb, get_flt, set_flt, (float *)&cm.a[AXIS_A].zero_backoff, A_ZERO_BACKOFF }, - - { "b","bam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_B].axis_mode, B_AXIS_MODE }, - { "b","bvm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_B].velocity_max, B_VELOCITY_MAX }, - { "b","bfr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_B].feedrate_max, B_FEEDRATE_MAX }, - { "b","btn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_B].travel_min, B_TRAVEL_MIN }, - { "b","btm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_B].travel_max, B_TRAVEL_MAX }, - { "b","bjm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_B].jerk_max, B_JERK_MAX }, - { "b","bjd",_fip, 0, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_B].junction_dev, B_JUNCTION_DEVIATION }, - { "b","bra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_B].radius, B_RADIUS }, - - { "c","cam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_C].axis_mode, C_AXIS_MODE }, - { "c","cvm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_C].velocity_max, C_VELOCITY_MAX }, - { "c","cfr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_C].feedrate_max, C_FEEDRATE_MAX }, - { "c","ctn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_C].travel_min, C_TRAVEL_MIN }, - { "c","ctm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_C].travel_max, C_TRAVEL_MAX }, - { "c","cjm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_C].jerk_max, C_JERK_MAX }, - { "c","cjd",_fip, 0, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_C].junction_dev, C_JUNCTION_DEVIATION }, - { "c","cra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_C].radius, C_RADIUS }, - - // PWM settings - { "p1","p1frq",_fip, 0, pwm_print_p1frq, get_flt, set_flt,(float *)&pwm.c[PWM_1].frequency, P1_PWM_FREQUENCY }, - { "p1","p1csl",_fip, 0, pwm_print_p1csl, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_speed_lo, P1_CW_SPEED_LO }, - { "p1","p1csh",_fip, 0, pwm_print_p1csh, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_speed_hi, P1_CW_SPEED_HI }, - { "p1","p1cpl",_fip, 3, pwm_print_p1cpl, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_phase_lo, P1_CW_PHASE_LO }, - { "p1","p1cph",_fip, 3, pwm_print_p1cph, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_phase_hi, P1_CW_PHASE_HI }, - { "p1","p1wsl",_fip, 0, pwm_print_p1wsl, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_speed_lo, P1_CCW_SPEED_LO }, - { "p1","p1wsh",_fip, 0, pwm_print_p1wsh, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_speed_hi, P1_CCW_SPEED_HI }, - { "p1","p1wpl",_fip, 3, pwm_print_p1wpl, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_phase_lo, P1_CCW_PHASE_LO }, - { "p1","p1wph",_fip, 3, pwm_print_p1wph, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_phase_hi, P1_CCW_PHASE_HI }, - { "p1","p1pof",_fip, 3, pwm_print_p1pof, get_flt, set_flt,(float *)&pwm.c[PWM_1].phase_off, P1_PWM_PHASE_OFF }, - - // Coordinate system offsets (G54-G59 and G92) - { "g54","g54x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_X], G54_X_OFFSET }, - { "g54","g54y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Y], G54_Y_OFFSET }, - { "g54","g54z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Z], G54_Z_OFFSET }, - { "g54","g54a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_A], G54_A_OFFSET }, - { "g54","g54b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_B], G54_B_OFFSET }, - { "g54","g54c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_C], G54_C_OFFSET }, - - { "g55","g55x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_X], G55_X_OFFSET }, - { "g55","g55y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Y], G55_Y_OFFSET }, - { "g55","g55z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Z], G55_Z_OFFSET }, - { "g55","g55a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_A], G55_A_OFFSET }, - { "g55","g55b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_B], G55_B_OFFSET }, - { "g55","g55c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_C], G55_C_OFFSET }, - - { "g56","g56x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_X], G56_X_OFFSET }, - { "g56","g56y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Y], G56_Y_OFFSET }, - { "g56","g56z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Z], G56_Z_OFFSET }, - { "g56","g56a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_A], G56_A_OFFSET }, - { "g56","g56b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_B], G56_B_OFFSET }, - { "g56","g56c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_C], G56_C_OFFSET }, - - { "g57","g57x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_X], G57_X_OFFSET }, - { "g57","g57y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Y], G57_Y_OFFSET }, - { "g57","g57z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Z], G57_Z_OFFSET }, - { "g57","g57a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_A], G57_A_OFFSET }, - { "g57","g57b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_B], G57_B_OFFSET }, - { "g57","g57c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_C], G57_C_OFFSET }, - - { "g58","g58x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_X], G58_X_OFFSET }, - { "g58","g58y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Y], G58_Y_OFFSET }, - { "g58","g58z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Z], G58_Z_OFFSET }, - { "g58","g58a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_A], G58_A_OFFSET }, - { "g58","g58b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_B], G58_B_OFFSET }, - { "g58","g58c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_C], G58_C_OFFSET }, - - { "g59","g59x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_X], G59_X_OFFSET }, - { "g59","g59y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Y], G59_Y_OFFSET }, - { "g59","g59z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Z], G59_Z_OFFSET }, - { "g59","g59a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_A], G59_A_OFFSET }, - { "g59","g59b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_B], G59_B_OFFSET }, - { "g59","g59c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_C], G59_C_OFFSET }, - - { "g92","g92x",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_X], 0 },// G92 handled differently - { "g92","g92y",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_Y], 0 }, - { "g92","g92z",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_Z], 0 }, - { "g92","g92a",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_A], 0 }, - { "g92","g92b",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_B], 0 }, - { "g92","g92c",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_C], 0 }, - - // Coordinate positions (G28, G30) - { "g28","g28x",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_X], 0 },// g28 handled differently - { "g28","g28y",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_Y], 0 }, - { "g28","g28z",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_Z], 0 }, - { "g28","g28a",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_A], 0 }, - { "g28","g28b",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_B], 0 }, - { "g28","g28c",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_C], 0 }, - - { "g30","g30x",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_X], 0 },// g30 handled differently - { "g30","g30y",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_Y], 0 }, - { "g30","g30z",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_Z], 0 }, - { "g30","g30a",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_A], 0 }, - { "g30","g30b",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_B], 0 }, - { "g30","g30c",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_C], 0 }, - - // this is a 128bit UUID for identifying a previously committed job state - { "jid","jida",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[0], 0}, - { "jid","jidb",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[1], 0}, - { "jid","jidc",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[2], 0}, - { "jid","jidd",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[3], 0}, - - // System parameters - { "sys","ja", _fipnc,0, cm_print_ja, get_flt, set_flu, (float *)&cm.junction_acceleration,JUNCTION_ACCELERATION }, - { "sys","ct", _fipnc,4, cm_print_ct, get_flt, set_flu, (float *)&cm.chordal_tolerance, CHORDAL_TOLERANCE }, - { "sys","sl", _fipn, 0, cm_print_sl, get_ui8, set_ui8, (float *)&cm.soft_limit_enable, SOFT_LIMIT_ENABLE }, - { "sys","st", _fipn, 0, sw_print_st, get_ui8, sw_set_st, (float *)&sw.switch_type, SWITCH_TYPE }, - { "sys","mt", _fipn, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.motor_power_timeout,MOTOR_IDLE_TIMEOUT}, - { "", "me", _f0, 0, tx_print_str, st_set_me, st_set_me, (float *)&cs.null, 0 }, - { "", "md", _f0, 0, tx_print_str, st_set_md, st_set_md, (float *)&cs.null, 0 }, - - { "sys","ej", _fipn, 0, js_print_ej, get_ui8, set_01, (float *)&cfg.comm_mode, COMM_MODE }, - { "sys","jv", _fipn, 0, js_print_jv, get_ui8, json_set_jv,(float *)&js.json_verbosity, JSON_VERBOSITY }, - { "sys","js", _fipn, 0, js_print_js, get_ui8, set_01, (float *)&js.json_syntax, JSON_SYNTAX_MODE }, - { "sys","tv", _fipn, 0, tx_print_tv, get_ui8, set_01, (float *)&txt.text_verbosity, TEXT_VERBOSITY }, - { "sys","qv", _fipn, 0, qr_print_qv, get_ui8, set_0123, (float *)&qr.queue_report_verbosity,QUEUE_REPORT_VERBOSITY }, - { "sys","sv", _fipn, 0, sr_print_sv, get_ui8, set_012, (float *)&sr.status_report_verbosity,STATUS_REPORT_VERBOSITY }, - { "sys","si", _fipn, 0, sr_print_si, get_int, sr_set_si, (float *)&sr.status_report_interval,STATUS_REPORT_INTERVAL_MS }, -// { "sys","spi", _fipn, 0, xio_print_spi,get_ui8, xio_set_spi,(float *)&xio.spi_state, 0 }, - - { "sys","ec", _fipn, 0, cfg_print_ec, get_ui8, set_ec, (float *)&cfg.enable_cr, COM_EXPAND_CR }, - { "sys","ee", _fipn, 0, cfg_print_ee, get_ui8, set_ee, (float *)&cfg.enable_echo, COM_ENABLE_ECHO }, - { "sys","ex", _fipn, 0, cfg_print_ex, get_ui8, set_ex, (float *)&cfg.enable_flow_control,COM_ENABLE_FLOW_CONTROL }, - { "sys","baud",_fn, 0, cfg_print_baud,get_ui8, set_baud, (float *)&cfg.usb_baud_rate, XIO_BAUD_115200 }, - { "sys","net", _fipn, 0, cfg_print_net, get_ui8, set_ui8, (float *)&cs.network_mode, NETWORK_MODE }, - - // switch state readouts -/* - { "ss","ss0", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[0], 0 }, - { "ss","ss1", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[1], 0 }, - { "ss","ss2", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[2], 0 }, - { "ss","ss3", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[3], 0 }, - { "ss","ss4", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[4], 0 }, - { "ss","ss5", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[5], 0 }, - { "ss","ss6", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[6], 0 }, - { "ss","ss7", _f0, 0, print_ss, get_ui8, set_nul, (float *)&sw.state[7], 0 }, -*/ - // NOTE: The ordering within the gcode defaults is important for token resolution - { "sys","gpl", _fipn, 0, cm_print_gpl, get_ui8, set_012, (float *)&cm.select_plane, GCODE_DEFAULT_PLANE }, - { "sys","gun", _fipn, 0, cm_print_gun, get_ui8, set_01, (float *)&cm.units_mode, GCODE_DEFAULT_UNITS }, - { "sys","gco", _fipn, 0, cm_print_gco, get_ui8, set_ui8, (float *)&cm.coord_system, GCODE_DEFAULT_COORD_SYSTEM }, - { "sys","gpa", _fipn, 0, cm_print_gpa, get_ui8, set_012, (float *)&cm.path_control, GCODE_DEFAULT_PATH_CONTROL }, - { "sys","gdi", _fipn, 0, cm_print_gdi, get_ui8, set_01, (float *)&cm.distance_mode,GCODE_DEFAULT_DISTANCE_MODE }, - { "", "gc", _f0, 0, tx_print_nul, gc_get_gc, gc_run_gc,(float *)&cs.null, 0 }, // gcode block - must be last in this group - - // "hidden" parameters (not in system group) -// { "", "ms", _fip, 0, cm_print_ms, get_flt, set_flt, (float *)&cm.estd_segment_usec, NOM_SEGMENT_USEC }, -// { "", "ml", _fipc,4, cm_print_ml, get_flt, set_flu, (float *)&cm.min_segment_len, MIN_LINE_LENGTH }, - { "", "ma", _fipc,4, cm_print_ma, get_flt, set_flu, (float *)&cm.arc_segment_len, ARC_SEGMENT_LENGTH }, - { "", "fd", _fip, 0, tx_print_ui8, get_ui8, set_01, (float *)&js.json_footer_depth, JSON_FOOTER_DEPTH }, - - // User defined data groups - { "uda","uda0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[0], USER_DATA_A0 }, - { "uda","uda1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[1], USER_DATA_A1 }, - { "uda","uda2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[2], USER_DATA_A2 }, - { "uda","uda3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[3], USER_DATA_A3 }, - - { "udb","udb0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[0], USER_DATA_B0 }, - { "udb","udb1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[1], USER_DATA_B1 }, - { "udb","udb2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[2], USER_DATA_B2 }, - { "udb","udb3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[3], USER_DATA_B3 }, - - { "udc","udc0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[0], USER_DATA_C0 }, - { "udc","udc1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[1], USER_DATA_C1 }, - { "udc","udc2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[2], USER_DATA_C2 }, - { "udc","udc3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[3], USER_DATA_C3 }, - - { "udd","udd0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[0], USER_DATA_D0 }, - { "udd","udd1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[1], USER_DATA_D1 }, - { "udd","udd2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[2], USER_DATA_D2 }, - { "udd","udd3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[3], USER_DATA_D3 }, - - // Diagnostic parameters + // Axis parameters + {"x","xam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_X].axis_mode, X_AXIS_MODE}, + {"x","xvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_X].velocity_max, X_VELOCITY_MAX}, + {"x","xfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_X].feedrate_max, X_FEEDRATE_MAX}, + {"x","xtn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_min, X_TRAVEL_MIN}, + {"x","xtm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_X].travel_max, X_TRAVEL_MAX}, + {"x","xjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_X].jerk_max, X_JERK_MAX}, + {"x","xjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_X].jerk_homing, X_JERK_HOMING}, + {"x","xjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_X].junction_dev, X_JUNCTION_DEVIATION}, + {"x","xsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[0], X_SWITCH_MODE_MIN}, + {"x","xsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[1], X_SWITCH_MODE_MAX}, + {"x","xsv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_X].search_velocity, X_SEARCH_VELOCITY}, + {"x","xlv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_X].latch_velocity, X_LATCH_VELOCITY}, + {"x","xlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_X].latch_backoff, X_LATCH_BACKOFF}, + {"x","xzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_X].zero_backoff, X_ZERO_BACKOFF}, + + {"y","yam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Y].axis_mode, Y_AXIS_MODE}, + {"y","yvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_Y].velocity_max, Y_VELOCITY_MAX}, + {"y","yfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_Y].feedrate_max, Y_FEEDRATE_MAX}, + {"y","ytn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_min, Y_TRAVEL_MIN}, + {"y","ytm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Y].travel_max, Y_TRAVEL_MAX}, + {"y","yjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_Y].jerk_max, Y_JERK_MAX}, + {"y","yjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_Y].jerk_homing, Y_JERK_HOMING}, + {"y","yjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_Y].junction_dev, Y_JUNCTION_DEVIATION}, + {"y","ysn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[2], Y_SWITCH_MODE_MIN}, + {"y","ysx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[3], Y_SWITCH_MODE_MAX}, + {"y","ysv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_Y].search_velocity, Y_SEARCH_VELOCITY}, + {"y","ylv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_Y].latch_velocity, Y_LATCH_VELOCITY}, + {"y","ylb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].latch_backoff, Y_LATCH_BACKOFF}, + {"y","yzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Y].zero_backoff, Y_ZERO_BACKOFF}, + + {"z","zam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_Z].axis_mode, Z_AXIS_MODE}, + {"z","zvm",_fipc, 0, cm_print_vm, get_flt, set_flu, (float *)&cm.a[AXIS_Z].velocity_max, Z_VELOCITY_MAX}, + {"z","zfr",_fipc, 0, cm_print_fr, get_flt, set_flu, (float *)&cm.a[AXIS_Z].feedrate_max, Z_FEEDRATE_MAX}, + {"z","ztn",_fipc, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_min, Z_TRAVEL_MIN}, + {"z","ztm",_fipc, 3, cm_print_tm, get_flt, set_flu, (float *)&cm.a[AXIS_Z].travel_max, Z_TRAVEL_MAX}, + {"z","zjm",_fipc, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_Z].jerk_max, Z_JERK_MAX}, + {"z","zjh",_fipc, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_Z].jerk_homing, Z_JERK_HOMING}, + {"z","zjd",_fipc, 4, cm_print_jd, get_flt, set_flu, (float *)&cm.a[AXIS_Z].junction_dev, Z_JUNCTION_DEVIATION}, + {"z","zsn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[4], Z_SWITCH_MODE_MIN}, + {"z","zsx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[5], Z_SWITCH_MODE_MAX}, + {"z","zsv",_fipc, 0, cm_print_sv, get_flt, set_flu, (float *)&cm.a[AXIS_Z].search_velocity, Z_SEARCH_VELOCITY}, + {"z","zlv",_fipc, 0, cm_print_lv, get_flt, set_flu, (float *)&cm.a[AXIS_Z].latch_velocity, Z_LATCH_VELOCITY}, + {"z","zlb",_fipc, 3, cm_print_lb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].latch_backoff, Z_LATCH_BACKOFF}, + {"z","zzb",_fipc, 3, cm_print_zb, get_flt, set_flu, (float *)&cm.a[AXIS_Z].zero_backoff, Z_ZERO_BACKOFF}, + + {"a","aam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_A].axis_mode, A_AXIS_MODE}, + {"a","avm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_A].velocity_max, A_VELOCITY_MAX}, + {"a","afr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_A].feedrate_max, A_FEEDRATE_MAX}, + {"a","atn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_A].travel_min, A_TRAVEL_MIN}, + {"a","atm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_A].travel_max, A_TRAVEL_MAX}, + {"a","ajm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_A].jerk_max, A_JERK_MAX}, + {"a","ajh",_fip, 0, cm_print_jh, get_flt, cm_set_xjh,(float *)&cm.a[AXIS_A].jerk_homing, A_JERK_HOMING}, + {"a","ajd",_fip, 4, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_A].junction_dev, A_JUNCTION_DEVIATION}, + {"a","ara",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_A].radius, A_RADIUS}, + {"a","asn",_fip, 0, cm_print_sn, get_ui8, sw_set_sw, (float *)&sw.mode[6], A_SWITCH_MODE_MIN}, + {"a","asx",_fip, 0, cm_print_sx, get_ui8, sw_set_sw, (float *)&sw.mode[7], A_SWITCH_MODE_MAX}, + {"a","asv",_fip, 0, cm_print_sv, get_flt, set_flt, (float *)&cm.a[AXIS_A].search_velocity, A_SEARCH_VELOCITY}, + {"a","alv",_fip, 0, cm_print_lv, get_flt, set_flt, (float *)&cm.a[AXIS_A].latch_velocity, A_LATCH_VELOCITY}, + {"a","alb",_fip, 3, cm_print_lb, get_flt, set_flt, (float *)&cm.a[AXIS_A].latch_backoff, A_LATCH_BACKOFF}, + {"a","azb",_fip, 3, cm_print_zb, get_flt, set_flt, (float *)&cm.a[AXIS_A].zero_backoff, A_ZERO_BACKOFF}, + + {"b","bam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_B].axis_mode, B_AXIS_MODE}, + {"b","bvm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_B].velocity_max, B_VELOCITY_MAX}, + {"b","bfr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_B].feedrate_max, B_FEEDRATE_MAX}, + {"b","btn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_B].travel_min, B_TRAVEL_MIN}, + {"b","btm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_B].travel_max, B_TRAVEL_MAX}, + {"b","bjm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_B].jerk_max, B_JERK_MAX}, + {"b","bjd",_fip, 0, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_B].junction_dev, B_JUNCTION_DEVIATION}, + {"b","bra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_B].radius, B_RADIUS}, + + {"c","cam",_fip, 0, cm_print_am, cm_get_am, cm_set_am, (float *)&cm.a[AXIS_C].axis_mode, C_AXIS_MODE}, + {"c","cvm",_fip, 0, cm_print_vm, get_flt, set_flt, (float *)&cm.a[AXIS_C].velocity_max, C_VELOCITY_MAX}, + {"c","cfr",_fip, 0, cm_print_fr, get_flt, set_flt, (float *)&cm.a[AXIS_C].feedrate_max, C_FEEDRATE_MAX}, + {"c","ctn",_fip, 3, cm_print_tn, get_flt, set_flu, (float *)&cm.a[AXIS_C].travel_min, C_TRAVEL_MIN}, + {"c","ctm",_fip, 3, cm_print_tm, get_flt, set_flt, (float *)&cm.a[AXIS_C].travel_max, C_TRAVEL_MAX}, + {"c","cjm",_fip, 0, cm_print_jm, get_flt, cm_set_xjm,(float *)&cm.a[AXIS_C].jerk_max, C_JERK_MAX}, + {"c","cjd",_fip, 0, cm_print_jd, get_flt, set_flt, (float *)&cm.a[AXIS_C].junction_dev, C_JUNCTION_DEVIATION}, + {"c","cra",_fipc, 3, cm_print_ra, get_flt, set_flt, (float *)&cm.a[AXIS_C].radius, C_RADIUS}, + + // PWM settings + {"p1","p1frq",_fip, 0, pwm_print_p1frq, get_flt, set_flt,(float *)&pwm.c[PWM_1].frequency, P1_PWM_FREQUENCY}, + {"p1","p1csl",_fip, 0, pwm_print_p1csl, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_speed_lo, P1_CW_SPEED_LO}, + {"p1","p1csh",_fip, 0, pwm_print_p1csh, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_speed_hi, P1_CW_SPEED_HI}, + {"p1","p1cpl",_fip, 3, pwm_print_p1cpl, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_phase_lo, P1_CW_PHASE_LO}, + {"p1","p1cph",_fip, 3, pwm_print_p1cph, get_flt, set_flt,(float *)&pwm.c[PWM_1].cw_phase_hi, P1_CW_PHASE_HI}, + {"p1","p1wsl",_fip, 0, pwm_print_p1wsl, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_speed_lo, P1_CCW_SPEED_LO}, + {"p1","p1wsh",_fip, 0, pwm_print_p1wsh, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_speed_hi, P1_CCW_SPEED_HI}, + {"p1","p1wpl",_fip, 3, pwm_print_p1wpl, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_phase_lo, P1_CCW_PHASE_LO}, + {"p1","p1wph",_fip, 3, pwm_print_p1wph, get_flt, set_flt,(float *)&pwm.c[PWM_1].ccw_phase_hi, P1_CCW_PHASE_HI}, + {"p1","p1pof",_fip, 3, pwm_print_p1pof, get_flt, set_flt,(float *)&pwm.c[PWM_1].phase_off, P1_PWM_PHASE_OFF}, + + // Coordinate system offsets (G54-G59 and G92) + {"g54","g54x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_X], G54_X_OFFSET}, + {"g54","g54y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Y], G54_Y_OFFSET}, + {"g54","g54z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_Z], G54_Z_OFFSET}, + {"g54","g54a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_A], G54_A_OFFSET}, + {"g54","g54b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_B], G54_B_OFFSET}, + {"g54","g54c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G54][AXIS_C], G54_C_OFFSET}, + + {"g55","g55x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_X], G55_X_OFFSET}, + {"g55","g55y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Y], G55_Y_OFFSET}, + {"g55","g55z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_Z], G55_Z_OFFSET}, + {"g55","g55a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_A], G55_A_OFFSET}, + {"g55","g55b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_B], G55_B_OFFSET}, + {"g55","g55c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G55][AXIS_C], G55_C_OFFSET}, + + {"g56","g56x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_X], G56_X_OFFSET}, + {"g56","g56y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Y], G56_Y_OFFSET}, + {"g56","g56z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_Z], G56_Z_OFFSET}, + {"g56","g56a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_A], G56_A_OFFSET}, + {"g56","g56b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_B], G56_B_OFFSET}, + {"g56","g56c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G56][AXIS_C], G56_C_OFFSET}, + + {"g57","g57x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_X], G57_X_OFFSET}, + {"g57","g57y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Y], G57_Y_OFFSET}, + {"g57","g57z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_Z], G57_Z_OFFSET}, + {"g57","g57a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_A], G57_A_OFFSET}, + {"g57","g57b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_B], G57_B_OFFSET}, + {"g57","g57c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G57][AXIS_C], G57_C_OFFSET}, + + {"g58","g58x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_X], G58_X_OFFSET}, + {"g58","g58y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Y], G58_Y_OFFSET}, + {"g58","g58z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_Z], G58_Z_OFFSET}, + {"g58","g58a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_A], G58_A_OFFSET}, + {"g58","g58b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_B], G58_B_OFFSET}, + {"g58","g58c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G58][AXIS_C], G58_C_OFFSET}, + + {"g59","g59x",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_X], G59_X_OFFSET}, + {"g59","g59y",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Y], G59_Y_OFFSET}, + {"g59","g59z",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_Z], G59_Z_OFFSET}, + {"g59","g59a",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_A], G59_A_OFFSET}, + {"g59","g59b",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_B], G59_B_OFFSET}, + {"g59","g59c",_fipc, 3, cm_print_cofs, get_flt, set_flu,(float *)&cm.offset[G59][AXIS_C], G59_C_OFFSET}, + + {"g92","g92x",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_X], 0},// G92 handled differently + {"g92","g92y",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_Y], 0}, + {"g92","g92z",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_Z], 0}, + {"g92","g92a",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_A], 0}, + {"g92","g92b",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_B], 0}, + {"g92","g92c",_fi, 3, cm_print_cofs, get_flt, set_nul,(float *)&cm.gmx.origin_offset[AXIS_C], 0}, + + // Coordinate positions (G28, G30) + {"g28","g28x",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_X], 0},// g28 handled differently + {"g28","g28y",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_Y], 0}, + {"g28","g28z",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_Z], 0}, + {"g28","g28a",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_A], 0}, + {"g28","g28b",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_B], 0}, + {"g28","g28c",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g28_position[AXIS_C], 0}, + + {"g30","g30x",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_X], 0},// g30 handled differently + {"g30","g30y",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_Y], 0}, + {"g30","g30z",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_Z], 0}, + {"g30","g30a",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_A], 0}, + {"g30","g30b",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_B], 0}, + {"g30","g30c",_fi, 3, cm_print_cpos, get_flt, set_nul,(float *)&cm.gmx.g30_position[AXIS_C], 0}, + + // this is a 128bit UUID for identifying a previously committed job state + {"jid","jida",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[0], 0}, + {"jid","jidb",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[1], 0}, + {"jid","jidc",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[2], 0}, + {"jid","jidd",_f0, 0, tx_print_nul, get_data, set_data, (float *)&cs.job_id[3], 0}, + + // System parameters + {"sys","ja", _fipnc,0, cm_print_ja, get_flt, set_flu, (float *)&cm.junction_acceleration, JUNCTION_ACCELERATION}, + {"sys","ct", _fipnc,4, cm_print_ct, get_flt, set_flu, (float *)&cm.chordal_tolerance, CHORDAL_TOLERANCE}, + {"sys","sl", _fipn, 0, cm_print_sl, get_ui8, set_ui8, (float *)&cm.soft_limit_enable, SOFT_LIMIT_ENABLE}, + {"sys","st", _fipn, 0, sw_print_st, get_ui8, sw_set_st, (float *)&sw.switch_type, SWITCH_TYPE}, + {"sys","mt", _fipn, 2, st_print_mt, get_flt, st_set_mt, (float *)&st_cfg.motor_power_timeout, MOTOR_IDLE_TIMEOUT}, + {"", "me", _f0, 0, tx_print_str, st_set_me, st_set_me, (float *)&cs.null, 0}, + {"", "md", _f0, 0, tx_print_str, st_set_md, st_set_md, (float *)&cs.null, 0}, + + {"sys","ej", _fipn, 0, js_print_ej, get_ui8, set_01, (float *)&cfg.comm_mode, COMM_MODE}, + {"sys","jv", _fipn, 0, js_print_jv, get_ui8, json_set_jv,(float *)&js.json_verbosity, JSON_VERBOSITY}, + {"sys","js", _fipn, 0, js_print_js, get_ui8, set_01, (float *)&js.json_syntax, JSON_SYNTAX_MODE}, + {"sys","tv", _fipn, 0, tx_print_tv, get_ui8, set_01, (float *)&txt.text_verbosity, TEXT_VERBOSITY}, + {"sys","qv", _fipn, 0, qr_print_qv, get_ui8, set_0123, (float *)&qr.queue_report_verbosity, QUEUE_REPORT_VERBOSITY}, + {"sys","sv", _fipn, 0, sr_print_sv, get_ui8, set_012, (float *)&sr.status_report_verbosity, STATUS_REPORT_VERBOSITY}, + {"sys","si", _fipn, 0, sr_print_si, get_int, sr_set_si, (float *)&sr.status_report_interval, STATUS_REPORT_INTERVAL_MS}, + + {"sys","ec", _fipn, 0, cfg_print_ec, get_ui8, set_ec, (float *)&cfg.enable_cr, COM_EXPAND_CR}, + {"sys","ee", _fipn, 0, cfg_print_ee, get_ui8, set_ee, (float *)&cfg.enable_echo, COM_ENABLE_ECHO}, + {"sys","ex", _fipn, 0, cfg_print_ex, get_ui8, set_ex, (float *)&cfg.enable_flow_control, COM_ENABLE_FLOW_CONTROL}, + {"sys","baud",_fn, 0, cfg_print_baud,get_ui8, set_baud, (float *)&cfg.usb_baud_rate, XIO_BAUD_115200}, + {"sys","net", _fipn, 0, cfg_print_net, get_ui8, set_ui8, (float *)&cs.network_mode, NETWORK_MODE}, + + // NOTE: The ordering within the gcode defaults is important for token resolution + {"sys","gpl", _fipn, 0, cm_print_gpl, get_ui8, set_012, (float *)&cm.select_plane, GCODE_DEFAULT_PLANE}, + {"sys","gun", _fipn, 0, cm_print_gun, get_ui8, set_01, (float *)&cm.units_mode, GCODE_DEFAULT_UNITS}, + {"sys","gco", _fipn, 0, cm_print_gco, get_ui8, set_ui8, (float *)&cm.coord_system, GCODE_DEFAULT_COORD_SYSTEM}, + {"sys","gpa", _fipn, 0, cm_print_gpa, get_ui8, set_012, (float *)&cm.path_control, GCODE_DEFAULT_PATH_CONTROL}, + {"sys","gdi", _fipn, 0, cm_print_gdi, get_ui8, set_01, (float *)&cm.distance_mode, GCODE_DEFAULT_DISTANCE_MODE}, + {"", "gc", _f0, 0, tx_print_nul, gc_get_gc, gc_run_gc,(float *)&cs.null, 0}, // gcode block - must be last in this group + + // "hidden" parameters (not in system group) + {"", "ma", _fipc,4, cm_print_ma, get_flt, set_flu, (float *)&cm.arc_segment_len, ARC_SEGMENT_LENGTH}, + {"", "fd", _fip, 0, tx_print_ui8, get_ui8, set_01, (float *)&js.json_footer_depth, JSON_FOOTER_DEPTH}, + + // User defined data groups + {"uda","uda0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[0], USER_DATA_A0}, + {"uda","uda1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[1], USER_DATA_A1}, + {"uda","uda2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[2], USER_DATA_A2}, + {"uda","uda3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_a[3], USER_DATA_A3}, + + {"udb","udb0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[0], USER_DATA_B0}, + {"udb","udb1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[1], USER_DATA_B1}, + {"udb","udb2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[2], USER_DATA_B2}, + {"udb","udb3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_b[3], USER_DATA_B3}, + + {"udc","udc0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[0], USER_DATA_C0}, + {"udc","udc1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[1], USER_DATA_C1}, + {"udc","udc2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[2], USER_DATA_C2}, + {"udc","udc3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_c[3], USER_DATA_C3}, + + {"udd","udd0", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[0], USER_DATA_D0}, + {"udd","udd1", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[1], USER_DATA_D1}, + {"udd","udd2", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[2], USER_DATA_D2}, + {"udd","udd3", _fip, 0, tx_print_int, get_data, set_data,(float *)&cfg.user_data_d[3], USER_DATA_D3}, + + // Diagnostic parameters #ifdef __DIAGNOSTIC_PARAMETERS - { "_te","_tex",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_X], 0 }, // X target endpoint - { "_te","_tey",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Y], 0 }, - { "_te","_tez",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Z], 0 }, - { "_te","_tea",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_A], 0 }, - { "_te","_teb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_B], 0 }, - { "_te","_tec",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_C], 0 }, - - { "_tr","_trx",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_X], 0 }, // X target runtime - { "_tr","_try",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Y], 0 }, - { "_tr","_trz",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Z], 0 }, - { "_tr","_tra",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_A], 0 }, - { "_tr","_trb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_B], 0 }, - { "_tr","_trc",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_C], 0 }, + {"_te","_tex",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_X], 0}, // X target endpoint + {"_te","_tey",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Y], 0}, + {"_te","_tez",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_Z], 0}, + {"_te","_tea",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_A], 0}, + {"_te","_teb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_B], 0}, + {"_te","_tec",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target[AXIS_C], 0}, + + {"_tr","_trx",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_X], 0}, // X target runtime + {"_tr","_try",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Y], 0}, + {"_tr","_trz",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_Z], 0}, + {"_tr","_tra",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_A], 0}, + {"_tr","_trb",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_B], 0}, + {"_tr","_trc",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.gm.target[AXIS_C], 0}, #if (MOTORS >= 1) - { "_ts","_ts1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_1], 0 }, // Motor 1 target steps - { "_ps","_ps1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_1], 0 }, // Motor 1 position steps - { "_cs","_cs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_1], 0 }, // Motor 1 commanded steps (delayed steps) - { "_es","_es1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_1], 0 }, // Motor 1 encoder steps - { "_xs","_xs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_1].corrected_steps, 0 }, // Motor 1 correction steps applied - { "_fe","_fe1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_1], 0 }, // Motor 1 following error in steps + {"_ts","_ts1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_1], 0}, // Motor 1 target steps + {"_ps","_ps1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_1], 0}, // Motor 1 position steps + {"_cs","_cs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_1], 0}, // Motor 1 commanded steps (delayed steps) + {"_es","_es1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_1], 0}, // Motor 1 encoder steps + {"_xs","_xs1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_1].corrected_steps, 0}, // Motor 1 correction steps applied + {"_fe","_fe1",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_1], 0}, // Motor 1 following error in steps #endif #if (MOTORS >= 2) - { "_ts","_ts2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_2], 0 }, - { "_ps","_ps2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_2], 0 }, - { "_cs","_cs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_2], 0 }, - { "_es","_es2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_2], 0 }, - { "_xs","_xs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_2].corrected_steps, 0 }, - { "_fe","_fe2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_2], 0 }, + {"_ts","_ts2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_2], 0}, + {"_ps","_ps2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_2], 0}, + {"_cs","_cs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_2], 0}, + {"_es","_es2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_2], 0}, + {"_xs","_xs2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_2].corrected_steps, 0}, + {"_fe","_fe2",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_2], 0}, #endif #if (MOTORS >= 3) - { "_ts","_ts3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_3], 0 }, - { "_ps","_ps3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_3], 0 }, - { "_cs","_cs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_3], 0 }, - { "_es","_es3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_3], 0 }, - { "_xs","_xs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_3].corrected_steps, 0 }, - { "_fe","_fe3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_3], 0 }, + {"_ts","_ts3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_3], 0}, + {"_ps","_ps3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_3], 0}, + {"_cs","_cs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_3], 0}, + {"_es","_es3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_3], 0}, + {"_xs","_xs3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_3].corrected_steps, 0}, + {"_fe","_fe3",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_3], 0}, #endif #if (MOTORS >= 4) - { "_ts","_ts4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_4], 0 }, - { "_ps","_ps4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_4], 0 }, - { "_cs","_cs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_4], 0 }, - { "_es","_es4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_4], 0 }, - { "_xs","_xs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_4].corrected_steps, 0 }, - { "_fe","_fe4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_4], 0 }, + {"_ts","_ts4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_4], 0}, + {"_ps","_ps4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_4], 0}, + {"_cs","_cs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_4], 0}, + {"_es","_es4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_4], 0}, + {"_xs","_xs4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_4].corrected_steps, 0}, + {"_fe","_fe4",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_4], 0}, #endif #if (MOTORS >= 5) - { "_ts","_ts5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_5], 0 }, - { "_ps","_ps5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_5], 0 }, - { "_cs","_cs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_5], 0 }, - { "_es","_es5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_5], 0 }, - { "_xs","_xs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_6].corrected_steps, 0 }, - { "_fe","_fe5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_5], 0 }, + {"_ts","_ts5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_5], 0}, + {"_ps","_ps5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_5], 0}, + {"_cs","_cs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_5], 0}, + {"_es","_es5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_5], 0}, + {"_xs","_xs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_6].corrected_steps, 0}, + {"_fe","_fe5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_5], 0}, #endif #if (MOTORS >= 6) - { "_ts","_ts6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_6], 0 }, - { "_ps","_ps6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_6], 0 }, - { "_cs","_cs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_6], 0 }, - { "_es","_es6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_6], 0 }, - { "_xs","_xs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_5].corrected_steps, 0 }, - { "_fe","_fe6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_6], 0 }, + {"_ts","_ts6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.target_steps[MOTOR_6], 0}, + {"_ps","_ps6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.position_steps[MOTOR_6], 0}, + {"_cs","_cs6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.commanded_steps[MOTOR_6], 0}, + {"_es","_es6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.encoder_steps[MOTOR_6], 0}, + {"_xs","_xs5",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&st_pre.mot[MOTOR_5].corrected_steps, 0}, + {"_fe","_fe6",_f0, 2, tx_print_flt, get_flt, set_nul,(float *)&mr.following_error[MOTOR_6], 0}, #endif - { "", "_dam",_f0, 0, tx_print_nul, cm_dam, cm_dam, (float *)&cs.null, 0 }, // dump active model -#endif // __DIAGNOSTIC_PARAMETERS - - // Persistence for status report - must be in sequence - // *** Count must agree with NV_STATUS_REPORT_LEN in config.h *** - { "","se00",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[0],0 }, - { "","se01",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[1],0 }, - { "","se02",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[2],0 }, - { "","se03",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[3],0 }, - { "","se04",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[4],0 }, - { "","se05",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[5],0 }, - { "","se06",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[6],0 }, - { "","se07",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[7],0 }, - { "","se08",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[8],0 }, - { "","se09",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[9],0 }, - { "","se10",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[10],0 }, - { "","se11",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[11],0 }, - { "","se12",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[12],0 }, - { "","se13",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[13],0 }, - { "","se14",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[14],0 }, - { "","se15",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[15],0 }, - { "","se16",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[16],0 }, - { "","se17",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[17],0 }, - { "","se18",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[18],0 }, - { "","se19",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[19],0 }, - { "","se20",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[20],0 }, - { "","se21",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[21],0 }, - { "","se22",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[22],0 }, - { "","se23",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[23],0 }, - { "","se24",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[24],0 }, - { "","se25",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[25],0 }, - { "","se26",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[26],0 }, - { "","se27",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[27],0 }, - { "","se28",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[28],0 }, - { "","se29",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[29],0 }, -// Count is 30, since se00 counts as one. - - // Group lookups - must follow the single-valued entries for proper sub-string matching - // *** Must agree with NV_COUNT_GROUPS below *** - // *** START COUNTING FROM HERE *** - { "","sys",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // system group - { "","p1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // PWM 1 group - - { "","1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // motor groups - { "","2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, + {"", "_dam",_f0, 0, tx_print_nul, cm_dam, cm_dam, (float *)&cs.null, 0}, // dump active model +#endif // __DIAGNOSTIC_PARAMETERS + + // Persistence for status report - must be in sequence + // *** Count must agree with NV_STATUS_REPORT_LEN in config.h *** + {"","se00",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[0],0}, + {"","se01",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[1],0}, + {"","se02",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[2],0}, + {"","se03",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[3],0}, + {"","se04",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[4],0}, + {"","se05",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[5],0}, + {"","se06",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[6],0}, + {"","se07",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[7],0}, + {"","se08",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[8],0}, + {"","se09",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[9],0}, + {"","se10",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[10],0}, + {"","se11",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[11],0}, + {"","se12",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[12],0}, + {"","se13",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[13],0}, + {"","se14",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[14],0}, + {"","se15",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[15],0}, + {"","se16",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[16],0}, + {"","se17",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[17],0}, + {"","se18",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[18],0}, + {"","se19",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[19],0}, + {"","se20",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[20],0}, + {"","se21",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[21],0}, + {"","se22",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[22],0}, + {"","se23",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[23],0}, + {"","se24",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[24],0}, + {"","se25",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[25],0}, + {"","se26",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[26],0}, + {"","se27",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[27],0}, + {"","se28",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[28],0}, + {"","se29",_fp, 0, tx_print_nul, get_int, set_int,(float *)&sr.status_report_list[29],0}, + // Count is 30, since se00 counts as one. + + // Group lookups - must follow the single-valued entries for proper sub-string matching + // *** Must agree with NV_COUNT_GROUPS below *** + // *** START COUNTING FROM HERE *** + {"","sys",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // system group + {"","p1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // PWM 1 group + + {"","1", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // motor groups + {"","2", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","3", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","4", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, #if (MOTORS >= 5) - { "","5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, + {"","5", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, #endif #if (MOTORS >= 6) - { "","6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, + {"","6", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, #endif - { "","x", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis groups - { "","y", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","z", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","a", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","b", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","c", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - -// { "","ss", _f0, 0, tx_print_nul, get_grp, set_nul,(float *)&cs.null,0 }, // switch states - { "","g54",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // coord offset groups - { "","g55",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g56",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g57",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g58",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g59",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, - { "","g92",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // origin offsets - { "","g28",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g28 home position - { "","g30",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // g30 home position - - { "","mpo",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // machine position group - { "","pos",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work position group - { "","ofs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // work offset group - { "","hom",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis homing state group - { "","prb",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // probing state group - { "","pwr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // motor power enagled group - { "","jog",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // axis jogging state group - { "","jid",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // job ID group - - { "","uda", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group - { "","udb", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group - { "","udc", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group - { "","udd", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // user data group + {"","x", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // axis groups + {"","y", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","z", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","a", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","b", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","c", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + + {"","g54",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // coord offset groups + {"","g55",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","g56",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","g57",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","g58",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","g59",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, + {"","g92",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // origin offsets + {"","g28",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // g28 home position + {"","g30",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // g30 home position + + {"","mpo",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // machine position group + {"","pos",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // work position group + {"","ofs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // work offset group + {"","hom",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // axis homing state group + {"","prb",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // probing state group + {"","pwr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // motor power enagled group + {"","jog",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // axis jogging state group + {"","jid",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // job ID group + + {"","uda", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group + {"","udb", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group + {"","udc", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group + {"","udd", _f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // user data group #ifdef __DIAGNOSTIC_PARAMETERS - { "","_te",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // target axis endpoint group - { "","_tr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // target axis runtime group - { "","_ts",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // target motor steps group - { "","_ps",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // position motor steps group - { "","_cs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // commanded motor steps group - { "","_es",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // encoder steps group - { "","_xs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // correction steps group - { "","_fe",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0 }, // following error group + {"","_te",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // target axis endpoint group + {"","_tr",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // target axis runtime group + {"","_ts",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // target motor steps group + {"","_ps",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // position motor steps group + {"","_cs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // commanded motor steps group + {"","_es",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // encoder steps group + {"","_xs",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // correction steps group + {"","_fe",_f0, 0, tx_print_nul, get_grp, set_grp,(float *)&cs.null,0}, // following error group #endif - // Uber-group (groups of groups, for text-mode displays only) - // *** Must agree with NV_COUNT_UBER_GROUPS below **** - { "", "m", _f0, 0, tx_print_nul, _do_motors, set_nul,(float *)&cs.null,0 }, - { "", "q", _f0, 0, tx_print_nul, _do_axes, set_nul,(float *)&cs.null,0 }, - { "", "o", _f0, 0, tx_print_nul, _do_offsets,set_nul,(float *)&cs.null,0 }, - { "", "$", _f0, 0, tx_print_nul, _do_all, set_nul,(float *)&cs.null,0 } + // Uber-group (groups of groups, for text-mode displays only) + // *** Must agree with NV_COUNT_UBER_GROUPS below **** + {"", "m", _f0, 0, tx_print_nul, _do_motors, set_nul,(float *)&cs.null,0}, + {"", "q", _f0, 0, tx_print_nul, _do_axes, set_nul,(float *)&cs.null,0}, + {"", "o", _f0, 0, tx_print_nul, _do_offsets,set_nul,(float *)&cs.null,0}, + {"", "$", _f0, 0, tx_print_nul, _do_all, set_nul,(float *)&cs.null,0} }; /***** Make sure these defines line up with any changes in the above table *****/ -#define NV_COUNT_UBER_GROUPS 4 // count of uber-groups, above -#define STANDARD_GROUPS 33 // count of standard groups, excluding diagnostic parameter groups +#define NV_COUNT_UBER_GROUPS 4 // count of uber-groups, above +#define STANDARD_GROUPS 33 // count of standard groups, excluding diagnostic parameter groups #if (MOTORS >= 5) -#define MOTOR_GROUP_5 1 +#define MOTOR_GROUP_5 1 #else -#define MOTOR_GROUP_5 0 +#define MOTOR_GROUP_5 0 #endif #if (MOTORS >= 6) -#define MOTOR_GROUP_6 1 +#define MOTOR_GROUP_6 1 #else -#define MOTOR_GROUP_6 0 +#define MOTOR_GROUP_6 0 #endif #ifdef __DIAGNOSTIC_PARAMETERS -#define DIAGNOSTIC_GROUPS 8 // count of diagnostic groups only +#define DIAGNOSTIC_GROUPS 8 // count of diagnostic groups only #else -#define DIAGNOSTIC_GROUPS 0 +#define DIAGNOSTIC_GROUPS 0 #endif -#define NV_COUNT_GROUPS (STANDARD_GROUPS + MOTOR_GROUP_5 + MOTOR_GROUP_6 + DIAGNOSTIC_GROUPS) +#define NV_COUNT_GROUPS (STANDARD_GROUPS + MOTOR_GROUP_5 + MOTOR_GROUP_6 + DIAGNOSTIC_GROUPS) /* */ #define NV_INDEX_MAX (sizeof cfgArray / sizeof(cfgItem_t)) -#define NV_INDEX_END_SINGLES (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS - NV_COUNT_GROUPS - NV_STATUS_REPORT_LEN) -#define NV_INDEX_START_GROUPS (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS - NV_COUNT_GROUPS) +#define NV_INDEX_END_SINGLES (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS - NV_COUNT_GROUPS - NV_STATUS_REPORT_LEN) +#define NV_INDEX_START_GROUPS (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS - NV_COUNT_GROUPS) #define NV_INDEX_START_UBER_GROUPS (NV_INDEX_MAX - NV_COUNT_UBER_GROUPS) /* */ -index_t nv_index_max() { return ( NV_INDEX_MAX );} -uint8_t nv_index_is_single(index_t index) { return ((index <= NV_INDEX_END_SINGLES) ? true : false);} -uint8_t nv_index_is_group(index_t index) { return (((index >= NV_INDEX_START_GROUPS) && (index < NV_INDEX_START_UBER_GROUPS)) ? true : false);} -uint8_t nv_index_lt_groups(index_t index) { return ((index <= NV_INDEX_START_GROUPS) ? true : false);} +index_t nv_index_max() {return NV_INDEX_MAX;} +uint8_t nv_index_is_single(index_t index) {return (index <= NV_INDEX_END_SINGLES ? true : false);} +uint8_t nv_index_is_group(index_t index) {return ((index >= NV_INDEX_START_GROUPS && (index < NV_INDEX_START_UBER_GROUPS)) ? true : false);} +uint8_t nv_index_lt_groups(index_t index) {return (index <= NV_INDEX_START_GROUPS ? true : false);} /***** APPLICATION SPECIFIC CONFIGS AND EXTENSIONS TO GENERIC FUNCTIONS *****/ @@ -705,184 +671,158 @@ uint8_t nv_index_lt_groups(index_t index) { return ((index <= NV_INDEX_START_GRO * Displays should convert back from internal canonical form to external form. */ -stat_t set_flu(nvObj_t *nv) -{ - if (cm_get_units_mode(MODEL) == INCHES) { // if in inches... - nv->value *= MM_PER_INCH; // convert to canonical millimeter units - } - *((float *)GET_TABLE_WORD(target)) = nv->value; // write value as millimeters or degrees - nv->precision = GET_TABLE_WORD(precision); - nv->valuetype = TYPE_FLOAT; - return(STAT_OK); +stat_t set_flu(nvObj_t *nv) { + if (cm_get_units_mode(MODEL) == INCHES) // if in inches... + nv->value *= MM_PER_INCH; // convert to canonical millimeter units + + *((float *)GET_TABLE_WORD(target)) = nv->value; // write value as millimeters or degrees + nv->precision = GET_TABLE_WORD(precision); + nv->valuetype = TYPE_FLOAT; + + return STAT_OK; } /* * preprocess_float() - pre-process floating point number for units display */ -void preprocess_float(nvObj_t *nv) -{ - if (isnan((double)nv->value) || isinf((double)nv->value)) return; // illegal float values - if (GET_TABLE_BYTE(flags) & F_CONVERT) { // unit conversion required? - if (cm_get_units_mode(MODEL) == INCHES) { - nv->value *= INCHES_PER_MM; - } - } +void preprocess_float(nvObj_t *nv) { + if (isnan((double)nv->value) || isinf((double)nv->value)) return; // illegal float values + + if (GET_TABLE_BYTE(flags) & F_CONVERT) { // unit conversion required? + if (cm_get_units_mode(MODEL) == INCHES) + nv->value *= INCHES_PER_MM; + } } /**** TinyG UberGroup Operations **************************************************** * Uber groups are groups of groups organized for convenience: - * - motors - group of all motor groups - * - axes - group of all axis groups - * - offsets - group of all offsets and stored positions - * - all - group of all groups + * - motors - group of all motor groups + * - axes - group of all axis groups + * - offsets - group of all offsets and stored positions + * - all - group of all groups * - * _do_group_list() - get and print all groups in the list (iteration) - * _do_motors() - get and print motor uber group 1-N - * _do_axes() - get and print axis uber group XYZABC - * _do_offsets() - get and print offset uber group G54-G59, G28, G30, G92 - * _do_all() - get and print all groups uber group + * _do_group_list() - get and print all groups in the list (iteration) + * _do_motors() - get and print motor uber group 1-N + * _do_axes() - get and print axis uber group XYZABC + * _do_offsets() - get and print offset uber group G54-G59, G28, G30, G92 + * _do_all() - get and print all groups uber group */ - -static stat_t _do_group_list(nvObj_t *nv, char list[][TOKEN_LEN+1]) // helper to print multiple groups in a list -{ - for (uint8_t i=0; i < NV_MAX_OBJECTS; i++) { - if (list[i][0] == NUL) - return (STAT_COMPLETE); - - nv_reset_nv_list(); - nv = nv_body; - strncpy(nv->token, list[i], TOKEN_LEN); - nv->index = nv_get_index((const char_t *)"", nv->token); -// nv->valuetype = TYPE_PARENT; - nv_get_nvObj(nv); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - } - return (STAT_COMPLETE); +static stat_t _do_group_list(nvObj_t *nv, char list[][TOKEN_LEN + 1]) { // helper to print multiple groups in a list + for (uint8_t i = 0; i < NV_MAX_OBJECTS; i++) { + if (list[i][0] == 0) return STAT_COMPLETE; + + nv_reset_nv_list(); + nv = nv_body; + strncpy(nv->token, list[i], TOKEN_LEN); + nv->index = nv_get_index((const char_t *)"", nv->token); + nv_get_nvObj(nv); + nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); + } + + return STAT_COMPLETE; } -static stat_t _do_motors(nvObj_t *nv) // print parameters for all motor groups -{ + +static stat_t _do_motors(nvObj_t *nv) { // print parameters for all motor groups #if MOTORS == 2 - char list[][TOKEN_LEN+1] = {"1","2",""}; // must have a terminating element + char list[][TOKEN_LEN+1] = {"1","2",""}; // must have a terminating element #endif #if MOTORS == 3 - char list[][TOKEN_LEN+1] = {"1","2","3",""}; // must have a terminating element + char list[][TOKEN_LEN+1] = {"1","2","3",""}; // must have a terminating element #endif #if MOTORS == 4 - char list[][TOKEN_LEN+1] = {"1","2","3","4",""}; // must have a terminating element + char list[][TOKEN_LEN+1] = {"1","2","3","4",""}; // must have a terminating element #endif #if MOTORS == 5 - char list[][TOKEN_LEN+1] = {"1","2","3","4","5",""}; // must have a terminating element + char list[][TOKEN_LEN+1] = {"1","2","3","4","5",""}; // must have a terminating element #endif #if MOTORS == 6 - char list[][TOKEN_LEN+1] = {"1","2","3","4","5","6",""}; // must have a terminating element + char list[][TOKEN_LEN+1] = {"1","2","3","4","5","6",""}; // must have a terminating element #endif - return (_do_group_list(nv, list)); + return _do_group_list(nv, list); } -static stat_t _do_axes(nvObj_t *nv) // print parameters for all axis groups -{ - char list[][TOKEN_LEN+1] = {"x","y","z","a","b","c",""}; // must have a terminating element - return (_do_group_list(nv, list)); + +static stat_t _do_axes(nvObj_t *nv) { // print parameters for all axis groups + char list[][TOKEN_LEN+1] = {"x","y","z","a","b","c",""}; // must have a terminating element + return _do_group_list(nv, list); } -static stat_t _do_offsets(nvObj_t *nv) // print offset parameters for G54-G59,G92, G28, G30 -{ - char list[][TOKEN_LEN+1] = {"g54","g55","g56","g57","g58","g59","g92","g28","g30",""}; // must have a terminating element - return (_do_group_list(nv, list)); + +static stat_t _do_offsets(nvObj_t *nv) { // print offset parameters for G54-G59,G92, G28, G30 + char list[][TOKEN_LEN+1] = {"g54","g55","g56","g57","g58","g59","g92","g28","g30",""}; // must have a terminating element + return _do_group_list(nv, list); } -static stat_t _do_all(nvObj_t *nv) // print all parameters -{ - strcpy(nv->token,"sys"); // print system group - get_grp(nv); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - _do_motors(nv); // print all motor groups - _do_axes(nv); // print all axis groups +static stat_t _do_all(nvObj_t *nv) { // print all parameters + strcpy(nv->token,"sys"); // print system group + get_grp(nv); + nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); + + _do_motors(nv); // print all motor groups + _do_axes(nv); // print all axis groups - strcpy(nv->token,"p1"); // print PWM group - get_grp(nv); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); + strcpy(nv->token,"p1"); // print PWM group + get_grp(nv); + nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - return (_do_offsets(nv)); // print all offsets + return _do_offsets(nv); // print all offsets } -/*********************************************************************************** - * CONFIGURATION AND INTERFACE FUNCTIONS - * Functions to get and set variables from the cfgArray table - * Most of these can be found in their respective modules. - ***********************************************************************************/ /**** COMMUNICATIONS FUNCTIONS ****************************************************** * set_ec() - enable CRLF on TX * set_ee() - enable character echo * set_ex() - enable XON/XOFF or RTS/CTS flow control - * set_baud() - set USB baud rate - * get_rx() - get bytes available in RX buffer - * - * The above assume USB is the std device + * set_baud() - set baud rate + * get_rx() - get bytes available in RX buffer */ +static stat_t _set_comm_helper(nvObj_t *nv, uint32_t yes, uint32_t no) { + if (fp_NOT_ZERO(nv->value)) xio_ctrl(XIO_DEV_USB, yes); + else xio_ctrl(XIO_DEV_USB, no); -static stat_t _set_comm_helper(nvObj_t *nv, uint32_t yes, uint32_t no) -{ - if (fp_NOT_ZERO(nv->value)) { - (void)xio_ctrl(XIO_DEV_USB, yes); - } else { - (void)xio_ctrl(XIO_DEV_USB, no); - } - return (STAT_OK); + return STAT_OK; } -static stat_t set_ec(nvObj_t *nv) // expand CR to CRLF on TX -{ - if (nv->value > true) - return (STAT_INPUT_VALUE_RANGE_ERROR); - cfg.enable_cr = (uint8_t)nv->value; - return(_set_comm_helper(nv, XIO_CRLF, XIO_NOCRLF)); -} -static stat_t set_ee(nvObj_t *nv) // enable character echo -{ - if (nv->value > true) - return (STAT_INPUT_VALUE_RANGE_ERROR); - cfg.enable_echo = (uint8_t)nv->value; - return(_set_comm_helper(nv, XIO_ECHO, XIO_NOECHO)); +static stat_t set_ec(nvObj_t *nv) { // expand CR to CRLF on TX + if (nv->value > true) return STAT_INPUT_VALUE_RANGE_ERROR; + cfg.enable_cr = (uint8_t)nv->value; + return _set_comm_helper(nv, XIO_CRLF, XIO_NOCRLF); } -static stat_t set_ex(nvObj_t *nv) // enable XON/XOFF or RTS/CTS flow control -{ - if (nv->value > FLOW_CONTROL_RTS) - return (STAT_INPUT_VALUE_RANGE_ERROR); - cfg.enable_flow_control = (uint8_t)nv->value; - return(_set_comm_helper(nv, XIO_XOFF, XIO_NOXOFF)); + +static stat_t set_ee(nvObj_t *nv) { // enable character echo + if (nv->value > true) return STAT_INPUT_VALUE_RANGE_ERROR; + cfg.enable_echo = (uint8_t)nv->value; + return _set_comm_helper(nv, XIO_ECHO, XIO_NOECHO); } -static stat_t get_rx(nvObj_t *nv) -{ - nv->value = (float)xio_get_usb_rx_free(); - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); + +static stat_t set_ex(nvObj_t *nv) { // enable XON/XOFF or RTS/CTS flow control + if (nv->value > FLOW_CONTROL_RTS) return STAT_INPUT_VALUE_RANGE_ERROR; + cfg.enable_flow_control = (uint8_t)nv->value; + return _set_comm_helper(nv, XIO_XOFF, XIO_NOXOFF); } -/* run_sx() - send XOFF, XON --- test only -static stat_t run_sx(nvObj_t *nv) -{ - xio_putc(XIO_DEV_USB, XOFF); - xio_putc(XIO_DEV_USB, XON); - return (STAT_OK); + +static stat_t get_rx(nvObj_t *nv) { + nv->value = (float)xio_get_usb_rx_free(); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; } -*/ + /* * set_baud() - set USB baud rate * - * See xio_usart.h for valid values. Works as a callback. - * The initial routine changes the baud config setting and sets a flag - * Then it posts a user message indicating the new baud rate - * Then it waits for the TX buffer to empty (so the message is sent) - * Then it performs the callback to apply the new baud rate + * See xio_usart.h for valid values. Works as a callback. + * The initial routine changes the baud config setting and sets a flag + * Then it posts a user message indicating the new baud rate + * Then it waits for the TX buffer to empty (so the message is sent) + * Then it performs the callback to apply the new baud rate */ static const char msg_baud0[] PROGMEM = "0"; static const char msg_baud1[] PROGMEM = "9600"; @@ -891,53 +831,53 @@ static const char msg_baud3[] PROGMEM = "38400"; static const char msg_baud4[] PROGMEM = "57600"; static const char msg_baud5[] PROGMEM = "115200"; static const char msg_baud6[] PROGMEM = "230400"; -static const char *const msg_baud[] PROGMEM = { msg_baud0, msg_baud1, msg_baud2, msg_baud3, msg_baud4, msg_baud5, msg_baud6 }; - -static stat_t set_baud(nvObj_t *nv) -{ - uint8_t baud = (uint8_t)nv->value; - if ((baud < 1) || (baud > 6)) { - nv_add_conditional_message((const char_t *)"*** WARNING *** Unsupported baud rate specified"); -// nv_add_conditional_message(PSTR("*** WARNING *** Unsupported baud rate specified")); - return (STAT_INPUT_VALUE_RANGE_ERROR); - } - cfg.usb_baud_rate = baud; - cfg.usb_baud_flag = true; - char_t message[NV_MESSAGE_LEN]; - sprintf_P(message, PSTR("*** NOTICE *** Resetting baud rate to %s"),GET_TEXT_ITEM(msg_baud, baud)); - nv_add_conditional_message(message); - return (STAT_OK); +static const char *const msg_baud[] PROGMEM = {msg_baud0, msg_baud1, msg_baud2, msg_baud3, msg_baud4, msg_baud5, msg_baud6}; + + +static stat_t set_baud(nvObj_t *nv) { + uint8_t baud = (uint8_t)nv->value; + if ((baud < 1) || (baud > 6)) { + nv_add_conditional_message((const char_t *)"*** WARNING *** Unsupported baud rate specified"); + return STAT_INPUT_VALUE_RANGE_ERROR; + } + + cfg.usb_baud_rate = baud; + cfg.usb_baud_flag = true; + char_t message[NV_MESSAGE_LEN]; + sprintf_P(message, PSTR("*** NOTICE *** Resetting baud rate to %s"),GET_TEXT_ITEM(msg_baud, baud)); + nv_add_conditional_message(message); + + return STAT_OK; } -stat_t set_baud_callback(void) -{ - if (cfg.usb_baud_flag == false) - return(STAT_NOOP); - cfg.usb_baud_flag = false; - xio_set_baud(XIO_DEV_USB, cfg.usb_baud_rate); - return (STAT_OK); + +stat_t set_baud_callback() { + if (cfg.usb_baud_flag == false) return STAT_NOOP; + cfg.usb_baud_flag = false; + xio_set_baud(XIO_DEV_USB, cfg.usb_baud_rate); + + return STAT_OK; } + /*********************************************************************************** * TEXT MODE SUPPORT * Functions to print variables from the cfgArray table ***********************************************************************************/ #ifdef __TEXT_MODE - -//static const char fmt_ic[] PROGMEM = "[ic] ignore CR or LF on RX%8d [0=off,1=CR,2=LF]\n"; static const char fmt_ec[] PROGMEM = "[ec] expand LF to CRLF on TX%6d [0=off,1=on]\n"; static const char fmt_ee[] PROGMEM = "[ee] enable echo%18d [0=off,1=on]\n"; static const char fmt_ex[] PROGMEM = "[ex] enable flow control%10d [0=off,1=XON/XOFF, 2=RTS/CTS]\n"; -static const char fmt_baud[] PROGMEM = "[baud] USB baud rate%15d [1=9600,2=19200,3=38400,4=57600,5=115200,6=230400]\n"; +static const char fmt_baud[] PROGMEM = "[baud] Baud rate%15d [1=9600,2=19200,3=38400,4=57600,5=115200,6=230400]\n"; static const char fmt_net[] PROGMEM = "[net] network mode%17d [0=master]\n"; static const char fmt_rx[] PROGMEM = "rx:%d\n"; -void cfg_print_ec(nvObj_t *nv) { text_print_ui8(nv, fmt_ec);} -void cfg_print_ee(nvObj_t *nv) { text_print_ui8(nv, fmt_ee);} -void cfg_print_ex(nvObj_t *nv) { text_print_ui8(nv, fmt_ex);} -void cfg_print_baud(nvObj_t *nv) { text_print_ui8(nv, fmt_baud);} -void cfg_print_net(nvObj_t *nv) { text_print_ui8(nv, fmt_net);} -void cfg_print_rx(nvObj_t *nv) { text_print_ui8(nv, fmt_rx);} +void cfg_print_ec(nvObj_t *nv) {text_print_ui8(nv, fmt_ec);} +void cfg_print_ee(nvObj_t *nv) {text_print_ui8(nv, fmt_ee);} +void cfg_print_ex(nvObj_t *nv) {text_print_ui8(nv, fmt_ex);} +void cfg_print_baud(nvObj_t *nv) {text_print_ui8(nv, fmt_baud);} +void cfg_print_net(nvObj_t *nv) {text_print_ui8(nv, fmt_net);} +void cfg_print_rx(nvObj_t *nv) {text_print_ui8(nv, fmt_rx);} #endif // __TEXT_MODE diff --git a/src/config_app.h b/src/config_app.h index ef29374..5b738e4 100755 --- a/src/config_app.h +++ b/src/config_app.h @@ -25,39 +25,39 @@ **** APPLICATION_SPECIFIC DEFINITIONS AND SETTINGS ******************************** ***********************************************************************************/ -enum nvType { // classification of commands - NV_TYPE_NULL = 0, - NV_TYPE_CONFIG, // configuration commands - NV_TYPE_GCODE, // gcode - NV_TYPE_REPORT, // SR, QR and any other report - NV_TYPE_MESSAGE, // nv object carries a message - NV_TYPE_LINENUM // nv object carries a gcode line number +enum nvType { // classification of commands + NV_TYPE_0 = 0, + NV_TYPE_CONFIG, // configuration commands + NV_TYPE_GCODE, // gcode + NV_TYPE_REPORT, // SR, QR and any other report + NV_TYPE_MESSAGE, // nv object carries a message + NV_TYPE_LINENUM // nv object carries a gcode line number }; /*********************************************************************************** **** APPLICATION_SPECIFIC CONFIG STRUCTURE(S) ************************************* ***********************************************************************************/ -typedef struct cfgParameters { // mostly communications variables at this point - uint16_t magic_start; // magic number to test memory integrity +typedef struct cfgParameters { // mostly communications variables at this point + uint16_t magic_start; // magic number to test memory integrity - // communications settings - uint8_t comm_mode; // TG_TEXT_MODE or TG_JSON_MODE - uint8_t enable_cr; // enable CR in CRFL expansion on TX - uint8_t enable_echo; // enable text-mode echo - uint8_t enable_flow_control; // enable XON/XOFF or RTS/CTS flow control -// uint8_t ignore_crlf; // ignore CR or LF on RX --- these 4 are shadow settings for XIO cntrl bits + // communications settings + uint8_t comm_mode; // TG_TEXT_MODE or TG_JSON_MODE + uint8_t enable_cr; // enable CR in CRFL expansion on TX + uint8_t enable_echo; // enable text-mode echo + uint8_t enable_flow_control; // enable XON/XOFF or RTS/CTS flow control +// uint8_t ignore_crlf; // ignore CR or LF on RX --- these 4 are shadow settings for XIO cntrl bits - uint8_t usb_baud_rate; // see xio_usart.h for XIO_BAUD values - uint8_t usb_baud_flag; // technically this belongs in the controller singleton + uint8_t usb_baud_rate; // see xio_usart.h for XIO_BAUD values + uint8_t usb_baud_flag; // technically this belongs in the controller singleton - // user-defined data groups - uint32_t user_data_a[4]; - uint32_t user_data_b[4]; - uint32_t user_data_c[4]; - uint32_t user_data_d[4]; + // user-defined data groups + uint32_t user_data_a[4]; + uint32_t user_data_b[4]; + uint32_t user_data_c[4]; + uint32_t user_data_d[4]; - uint16_t magic_end; + uint16_t magic_end; } cfgParameters_t; extern cfgParameters_t cfg; @@ -66,7 +66,7 @@ extern cfgParameters_t cfg; * Functions to get and set variables from the cfgArray table ***********************************************************************************/ -stat_t set_baud_callback(void); +stat_t set_baud_callback(); // job config void job_print_job(nvObj_t *nv); @@ -81,21 +81,21 @@ uint8_t job_report_callback(); #ifdef __TEXT_MODE - void cfg_print_ec(nvObj_t *nv); - void cfg_print_ee(nvObj_t *nv); - void cfg_print_ex(nvObj_t *nv); - void cfg_print_baud(nvObj_t *nv); - void cfg_print_net(nvObj_t *nv); - void cfg_print_rx(nvObj_t *nv); + void cfg_print_ec(nvObj_t *nv); + void cfg_print_ee(nvObj_t *nv); + void cfg_print_ex(nvObj_t *nv); + void cfg_print_baud(nvObj_t *nv); + void cfg_print_net(nvObj_t *nv); + void cfg_print_rx(nvObj_t *nv); #else - #define cfg_print_ec tx_print_stub - #define cfg_print_ee tx_print_stub - #define cfg_print_ex tx_print_stub - #define cfg_print_baud tx_print_stub - #define cfg_print_net tx_print_stub - #define cfg_print_rx tx_print_stub + #define cfg_print_ec tx_print_stub + #define cfg_print_ee tx_print_stub + #define cfg_print_ex tx_print_stub + #define cfg_print_baud tx_print_stub + #define cfg_print_net tx_print_stub + #define cfg_print_rx tx_print_stub #endif // __TEXT_MODE diff --git a/src/controller.c b/src/controller.c index b692de3..37d9123 100755 --- a/src/controller.c +++ b/src/controller.c @@ -26,8 +26,8 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "controller.h" #include "json_parser.h" #include "text_parser.h" @@ -37,79 +37,62 @@ #include "planner.h" #include "stepper.h" +#include "gpio.h" +#include "switch.h" #include "encoder.h" #include "hardware.h" -#include "switch.h" -#include "gpio.h" #include "report.h" #include "help.h" #include "util.h" -#include "xio.h" +#include "xio/xio.h" -/*********************************************************************************** - **** STRUCTURE ALLOCATIONS ********************************************************* - ***********************************************************************************/ +controller_t cs; // controller state structure -controller_t cs; // controller state structure +static stat_t _shutdown_idler(); +static stat_t _normal_idler(); +static stat_t _limit_switch_handler(); +static stat_t _system_assertions(); +static stat_t _sync_to_planner(); +static stat_t _sync_to_tx_buffer(); +static stat_t _command_dispatch(); -/*********************************************************************************** - **** STATICS AND LOCALS *********************************************************** - ***********************************************************************************/ +// prep for export to other modules: +stat_t hardware_hard_reset_handler(); +stat_t hardware_bootloader_handler(); -static void _controller_HSM(void); -static stat_t _shutdown_idler(void); -static stat_t _normal_idler(void); -static stat_t _limit_switch_handler(void); -static stat_t _system_assertions(void); -static stat_t _sync_to_planner(void); -static stat_t _sync_to_tx_buffer(void); -static stat_t _command_dispatch(void); -// prep for export to other modules: -stat_t hardware_hard_reset_handler(void); -stat_t hardware_bootloader_handler(void); +void controller_init(uint8_t std_in, uint8_t std_out, uint8_t std_err) { + memset(&cs, 0, sizeof(controller_t)); // clear all values, job_id's, pointers and status + controller_init_assertions(); -/*********************************************************************************** - **** CODE ************************************************************************* - ***********************************************************************************/ -/* - * controller_init() - controller init - */ + cs.fw_build = TINYG_FIRMWARE_BUILD; + cs.fw_version = TINYG_FIRMWARE_VERSION; + cs.hw_platform = TINYG_HARDWARE_PLATFORM; // NB: HW version is set from EEPROM -void controller_init(uint8_t std_in, uint8_t std_out, uint8_t std_err) -{ - memset(&cs, 0, sizeof(controller_t)); // clear all values, job_id's, pointers and status - controller_init_assertions(); - - cs.fw_build = TINYG_FIRMWARE_BUILD; - cs.fw_version = TINYG_FIRMWARE_VERSION; - cs.hw_platform = TINYG_HARDWARE_PLATFORM; // NB: HW version is set from EEPROM - - cs.state = CONTROLLER_STARTUP; // ready to run startup lines - xio_set_stdin(std_in); - xio_set_stdout(std_out); - xio_set_stderr(std_err); - cs.default_src = std_in; - tg_set_primary_source(cs.default_src); + cs.state = CONTROLLER_STARTUP; // ready to run startup lines + xio_set_stdin(std_in); + xio_set_stdout(std_out); + xio_set_stderr(std_err); + cs.default_src = std_in; + tg_set_primary_source(cs.default_src); } -/* - * controller_init_assertions() - * controller_test_assertions() - check memory integrity of controller - */ -void controller_init_assertions() -{ - cs.magic_start = MAGICNUM; - cs.magic_end = MAGICNUM; +/// controller_test_assertions() - check memory integrity of controller +void controller_init_assertions() { + cs.magic_start = MAGICNUM; + cs.magic_end = MAGICNUM; } -stat_t controller_test_assertions() -{ - if ((cs.magic_start != MAGICNUM) || (cs.magic_end != MAGICNUM)) return (STAT_CONTROLLER_ASSERTION_FAILURE); - return (STAT_OK); + +stat_t controller_test_assertions() { + if ((cs.magic_start != MAGICNUM) || (cs.magic_end != MAGICNUM)) + return STAT_CONTROLLER_ASSERTION_FAILURE; + + return STAT_OK; } + /* * controller_run() - MAIN LOOP - top-level controller * @@ -128,124 +111,107 @@ stat_t controller_test_assertions() * * A routine that had no action (i.e. is OFF or idle) should return STAT_NOOP */ - -void controller_run() -{ - while (true) { - _controller_HSM(); - } +void controller_run() { +#define DISPATCH(func) if (func == STAT_EAGAIN) return; + + // Interrupt Service Routines are the highest priority controller functions + // See hardware.h for a list of ISRs and their priorities. + + // Kernel level ISR handlers, flags are set in ISRs, order is important + DISPATCH(hw_hard_reset_handler()); // 1. handle hard reset requests + DISPATCH(hw_bootloader_handler()); // 2. handle requests to enter bootloader + DISPATCH(_shutdown_idler()); // 3. idle in shutdown state + DISPATCH(_limit_switch_handler()); // 4. limit switch has been thrown + DISPATCH(cm_feedhold_sequencing_callback()); // 5a. feedhold state machine runner + DISPATCH(mp_plan_hold_callback()); // 5b. plan a feedhold from line runtime + DISPATCH(_system_assertions()); // 6. system integrity assertions + + // Planner hierarchy for gcode and cycles + DISPATCH(st_motor_power_callback()); // stepper motor power sequencing + DISPATCH(sr_status_report_callback()); // conditionally send status report + DISPATCH(qr_queue_report_callback()); // conditionally send queue report + DISPATCH(rx_report_callback()); // conditionally send rx report + DISPATCH(cm_arc_callback()); // arc generation runs behind lines + DISPATCH(cm_homing_callback()); // G28.2 continuation + DISPATCH(cm_jogging_callback()); // jog function + DISPATCH(cm_probe_callback()); // G38.2 continuation + DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle + + // Command readers and parsers + DISPATCH(_sync_to_planner()); // ensure there is at least one free buffer in planning queue + DISPATCH(_sync_to_tx_buffer()); // sync with TX buffer (pseudo-blocking) + DISPATCH(set_baud_callback()); // perform baud rate update (must be after TX sync) + DISPATCH(_command_dispatch()); // read and execute next command + DISPATCH(_normal_idler()); // blink LEDs slowly to show everything is OK } -#define DISPATCH(func) if (func == STAT_EAGAIN) return; -static void _controller_HSM() -{ -//----- Interrupt Service Routines are the highest priority controller functions ----// -// See hardware.h for a list of ISRs and their priorities. -// -//----- kernel level ISR handlers ----(flags are set in ISRs)------------------------// - // Order is important: - DISPATCH(hw_hard_reset_handler()); // 1. handle hard reset requests - DISPATCH(hw_bootloader_handler()); // 2. handle requests to enter bootloader - DISPATCH(_shutdown_idler()); // 3. idle in shutdown state -// DISPATCH( poll_switches()); // 4. run a switch polling cycle - DISPATCH(_limit_switch_handler()); // 5. limit switch has been thrown - - DISPATCH(cm_feedhold_sequencing_callback());// 6a. feedhold state machine runner - DISPATCH(mp_plan_hold_callback()); // 6b. plan a feedhold from line runtime - DISPATCH(_system_assertions()); // 7. system integrity assertions - -//----- planner hierarchy for gcode and cycles ---------------------------------------// - - DISPATCH(st_motor_power_callback()); // stepper motor power sequencing -// DISPATCH(switch_debounce_callback()); // debounce switches - DISPATCH(sr_status_report_callback()); // conditionally send status report - DISPATCH(qr_queue_report_callback()); // conditionally send queue report - DISPATCH(rx_report_callback()); // conditionally send rx report - DISPATCH(cm_arc_callback()); // arc generation runs behind lines - DISPATCH(cm_homing_callback()); // G28.2 continuation - DISPATCH(cm_jogging_callback()); // jog function - DISPATCH(cm_probe_callback()); // G38.2 continuation - DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle - -//----- command readers and parsers --------------------------------------------------// - - DISPATCH(_sync_to_planner()); // ensure there is at least one free buffer in planning queue - DISPATCH(_sync_to_tx_buffer()); // sync with TX buffer (pseudo-blocking) - DISPATCH(set_baud_callback()); // perform baud rate update (must be after TX sync) - DISPATCH(_command_dispatch()); // read and execute next command - DISPATCH(_normal_idler()); // blink LEDs slowly to show everything is OK -} /***************************************************************************** * _command_dispatch() - dispatch line received from active input device * - * Reads next command line and dispatches to relevant parser or action - * Accepts commands if the move queue has room - EAGAINS if it doesn't - * Manages cutback to serial input from file devices (EOF) - * Also responsible for prompts and for flow control + * Reads next command line and dispatches to relevant parser or action + * Accepts commands if the move queue has room - EAGAINS if it doesn't + * Manages cutback to serial input from file devices (EOF) + * Also responsible for prompts and for flow control */ - -static stat_t _command_dispatch() -{ - stat_t status; - - // read input line or return if not a completed line - // xio_gets() is a non-blocking workalike of fgets() - while (true) { - if ((status = xio_gets(cs.primary_src, cs.in_buf, sizeof(cs.in_buf))) == STAT_OK) { - cs.bufp = cs.in_buf; - break; - } - // handle end-of-file from file devices - if (status == STAT_EOF) { // EOF can come from file devices only - if (cfg.comm_mode == TEXT_MODE) { - fprintf_P(stderr, PSTR("End of command file\n")); - } else { - rpt_exception(STAT_EOF); // not really an exception - } - tg_reset_source(); // reset to default source - } - return (status); // Note: STAT_EAGAIN, errors, etc. will drop through - } - - // set up the buffers - cs.linelen = strlen(cs.in_buf)+1; // linelen only tracks primary input - strncpy(cs.saved_buf, cs.bufp, SAVED_BUFFER_LEN-1); // save input buffer for reporting - - // dispatch the new text line - switch (toupper(*cs.bufp)) { // first char - - case '!': { cm_request_feedhold(); break; } // include for diagnostics - case '%': { cm_request_queue_flush(); break; } - case '~': { cm_request_cycle_start(); break; } - - case NUL: { // blank line (just a CR) - if (cfg.comm_mode != JSON_MODE) { - text_response(STAT_OK, cs.saved_buf); - } - break; - } - case '$': case '?': case 'H': { // text mode input - cfg.comm_mode = TEXT_MODE; - text_response(text_parser(cs.bufp), cs.saved_buf); - break; - } - case '{': { // JSON input - cfg.comm_mode = JSON_MODE; - json_parser(cs.bufp); - break; - } - default: { // anything else must be Gcode - if (cfg.comm_mode == JSON_MODE) { // run it as JSON... - strncpy(cs.out_buf, cs.bufp, INPUT_BUFFER_LEN -8); // use out_buf as temp - sprintf((char *)cs.bufp,"{\"gc\":\"%s\"}\n", (char *)cs.out_buf); // '-8' is used for JSON chars - json_parser(cs.bufp); - } else { //...or run it as text - text_response(gc_gcode_parser(cs.bufp), cs.saved_buf); - } - } - } - return (STAT_OK); +static stat_t _command_dispatch() { + stat_t status; + + // read input line or return if not a completed line + // xio_gets() is a non-blocking workalike of fgets() + while (true) { + if ((status = xio_gets(cs.primary_src, cs.in_buf, sizeof(cs.in_buf))) == STAT_OK) { + cs.bufp = cs.in_buf; + break; + } + + // handle end-of-file from file devices + if (status == STAT_EOF) { // EOF can come from file devices only + if (cfg.comm_mode == TEXT_MODE) + fprintf_P(stderr, PSTR("End of command file\n")); + else rpt_exception(STAT_EOF); // not really an exception + + tg_reset_source(); // reset to default source + } + + return status; // Note: STAT_EAGAIN, errors, etc. will drop through + } + + // set up the buffers + cs.linelen = strlen(cs.in_buf)+1; // linelen only tracks primary input + strncpy(cs.saved_buf, cs.bufp, SAVED_BUFFER_LEN-1); // save input buffer for reporting + + // dispatch the new text line + switch (toupper(*cs.bufp)) { // first char + case '!': cm_request_feedhold(); break; // include for diagnostics + case '%': cm_request_queue_flush(); break; + case '~': cm_request_cycle_start(); break; + + case 0: // blank line (just a CR) + if (cfg.comm_mode != JSON_MODE) + text_response(STAT_OK, cs.saved_buf); + break; + + case '$': case '?': case 'H': // text mode input + cfg.comm_mode = TEXT_MODE; + text_response(text_parser(cs.bufp), cs.saved_buf); + break; + + case '{': // JSON input + cfg.comm_mode = JSON_MODE; + json_parser(cs.bufp); + break; + + default: // anything else must be Gcode + if (cfg.comm_mode == JSON_MODE) { // run it as JSON... + strncpy(cs.out_buf, cs.bufp, INPUT_BUFFER_LEN -8); // use out_buf as temp + sprintf((char *)cs.bufp,"{\"gc\":\"%s\"}\n", (char *)cs.out_buf); // '-8' is used for JSON chars + json_parser(cs.bufp); + + } else text_response(gc_gcode_parser(cs.bufp), cs.saved_buf); //...or run it as text + } + + return STAT_OK; } @@ -254,31 +220,31 @@ static stat_t _command_dispatch() * _shutdown_idler() - blink rapidly and prevent further activity from occurring * _normal_idler() - blink Indicator LED slowly to show everything is OK * - * Shutdown idler flashes indicator LED rapidly to show everything is not OK. - * Shutdown idler returns EAGAIN causing the control loop to never advance beyond - * this point. It's important that the reset handler is still called so a SW reset - * (ctrl-x) or bootloader request can be processed. + * Shutdown idler flashes indicator LED rapidly to show everything is not OK. + * Shutdown idler returns EAGAIN causing the control loop to never advance beyond + * this point. It's important that the reset handler is still called so a SW reset + * (ctrl-x) or bootloader request can be processed. */ -static stat_t _shutdown_idler() -{ - if (cm_get_machine_state() != MACHINE_SHUTDOWN) { return (STAT_OK);} +static stat_t _shutdown_idler() { + if (cm_get_machine_state() != MACHINE_SHUTDOWN) return STAT_OK; - if (SysTickTimer_getValue() > cs.led_timer) { - cs.led_timer = SysTickTimer_getValue() + LED_ALARM_TIMER; - IndicatorLed_toggle(); - } - return (STAT_EAGAIN); // EAGAIN prevents any lower-priority actions from running + if (SysTickTimer_getValue() > cs.led_timer) { + cs.led_timer = SysTickTimer_getValue() + LED_ALARM_TIMER; + IndicatorLed_toggle(); + } + + return STAT_EAGAIN; // EAGAIN prevents any lower-priority actions from running } -static stat_t _normal_idler() -{ - return (STAT_OK); + +static stat_t _normal_idler() { + return STAT_OK; } /* - * tg_reset_source() - reset source to default input device (see note) - * tg_set_primary_source() - set current primary input source + * tg_reset_source() - reset source to default input device (see note) + * tg_set_primary_source() - set current primary input source * tg_set_secondary_source() - set current primary input source * * Note: Once multiple serial devices are supported reset_source() should @@ -286,69 +252,51 @@ static stat_t _normal_idler() * and other messages are sent to the active device. */ -void tg_reset_source() { tg_set_primary_source(cs.default_src);} -void tg_set_primary_source(uint8_t dev) { cs.primary_src = dev;} -void tg_set_secondary_source(uint8_t dev) { cs.secondary_src = dev;} +void tg_reset_source() {tg_set_primary_source(cs.default_src);} +void tg_set_primary_source(uint8_t dev) {cs.primary_src = dev;} +void tg_set_secondary_source(uint8_t dev) {cs.secondary_src = dev;} /* * _sync_to_tx_buffer() - return eagain if TX queue is backed up * _sync_to_planner() - return eagain if planner is not ready for a new command * _sync_to_time() - return eagain if planner is not ready for a new command */ -static stat_t _sync_to_tx_buffer() -{ - if ((xio_get_tx_bufcount_usart(ds[XIO_DEV_USB].x) >= XOFF_TX_LO_WATER_MARK)) { - return (STAT_EAGAIN); - } - return (STAT_OK); -} +static stat_t _sync_to_tx_buffer() { + if ((xio_get_tx_bufcount_usart(ds[XIO_DEV_USB].x) >= XOFF_TX_LO_WATER_MARK)) + return STAT_EAGAIN; -static stat_t _sync_to_planner() -{ - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) { // allow up to N planner buffers for this line - return (STAT_EAGAIN); - } - return (STAT_OK); + return STAT_OK; } -/* -static stat_t _sync_to_time() -{ - if (cs.sync_to_time_time == 0) { // initial pass - cs.sync_to_time_time = SysTickTimer_getValue() + 100; //ms - return (STAT_OK); - } - if (SysTickTimer_getValue() < cs.sync_to_time_time) { - return (STAT_EAGAIN); - } - return (STAT_OK); + +static stat_t _sync_to_planner() { + if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) // allow up to N planner buffers for this line + return STAT_EAGAIN; + + return STAT_OK; } -*/ -/* - * _limit_switch_handler() - shut down system if limit switch fired - */ -static stat_t _limit_switch_handler(void) -{ - if (cm_get_machine_state() == MACHINE_ALARM) { return (STAT_NOOP);} - if (get_limit_switch_thrown() == false) return (STAT_NOOP); - return(cm_hard_alarm(STAT_LIMIT_SWITCH_HIT)); - return (STAT_OK); + +/// _limit_switch_handler() - shut down system if limit switch fired +static stat_t _limit_switch_handler() { + if (cm_get_machine_state() == MACHINE_ALARM) return STAT_NOOP; + if (!get_limit_switch_thrown()) return STAT_NOOP; + + return cm_hard_alarm(STAT_LIMIT_SWITCH_HIT); } -/* - * _system_assertions() - check memory integrity and other assertions - */ -#define emergency___everybody_to_get_from_street(a) if((status_code=a) != STAT_OK) return (cm_hard_alarm(status_code)); - -stat_t _system_assertions() -{ - emergency___everybody_to_get_from_street(config_test_assertions()); - emergency___everybody_to_get_from_street(controller_test_assertions()); - emergency___everybody_to_get_from_street(canonical_machine_test_assertions()); - emergency___everybody_to_get_from_street(planner_test_assertions()); - emergency___everybody_to_get_from_street(stepper_test_assertions()); - emergency___everybody_to_get_from_street(encoder_test_assertions()); - emergency___everybody_to_get_from_street(xio_test_assertions()); - return (STAT_OK); + +/// _system_assertions() - check memory integrity and other assertions +stat_t _system_assertions() { +#define system_assert(a) if ((status_code = a) != STAT_OK) return cm_hard_alarm(status_code); + + system_assert(config_test_assertions()); + system_assert(controller_test_assertions()); + system_assert(canonical_machine_test_assertions()); + system_assert(planner_test_assertions()); + system_assert(stepper_test_assertions()); + system_assert(encoder_test_assertions()); + system_assert(xio_test_assertions()); + + return STAT_OK; } diff --git a/src/controller.h b/src/controller.h index f18a530..dfe0e7d 100755 --- a/src/controller.h +++ b/src/controller.h @@ -28,72 +28,72 @@ #ifndef CONTROLLER_H_ONCE #define CONTROLLER_H_ONCE -#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) -#define SAVED_BUFFER_LEN 100 // saved buffer size (for reporting only) -#define OUTPUT_BUFFER_LEN 512 // text buffer size +#define INPUT_BUFFER_LEN 255 // text buffer size (255 max) +#define SAVED_BUFFER_LEN 100 // saved buffer size (for reporting only) +#define OUTPUT_BUFFER_LEN 512 // text buffer size // see also: tinyg.h MESSAGE_LEN and config.h NV_ lengths -#define LED_NORMAL_TIMER 1000 // blink rate for normal operation (in ms) -#define LED_ALARM_TIMER 100 // blink rate for alarm state (in ms) +#define LED_NORMAL_TIMER 1000 // blink rate for normal operation (in ms) +#define LED_ALARM_TIMER 100 // blink rate for alarm state (in ms) -typedef struct controllerSingleton { // main TG controller struct - magic_t magic_start; // magic number to test memory integrity - uint8_t state; // controller state - float null; // dumping ground for items with no target - float fw_build; // tinyg firmware build number - float fw_version; // tinyg firmware version number - float hw_platform; // tinyg hardware compatibility - platform type - float hw_version; // tinyg hardware compatibility - platform revision +typedef struct controllerSingleton { // main TG controller struct + magic_t magic_start; // magic number to test memory integrity + uint8_t state; // controller state + float null; // dumping ground for items with no target + float fw_build; // tinyg firmware build number + float fw_version; // tinyg firmware version number + float hw_platform; // tinyg hardware compatibility - platform type + float hw_version; // tinyg hardware compatibility - platform revision - // communications state variables - uint8_t primary_src; // primary input source device - uint8_t secondary_src; // secondary input source device - uint8_t default_src; // default source device - uint8_t network_mode; // 0=master, 1=repeater, 2=slave + // communications state variables + uint8_t primary_src; // primary input source device + uint8_t secondary_src; // secondary input source device + uint8_t default_src; // default source device + uint8_t network_mode; // 0=master, 1=repeater, 2=slave - uint16_t linelen; // length of currently processing line - uint16_t read_index; // length of line being read + uint16_t linelen; // length of currently processing line + uint16_t read_index; // length of line being read - // system state variables - uint8_t led_state; // LEGACY // 0=off, 1=on - int32_t led_counter; // LEGACY // a convenience for flashing an LED - uint32_t led_timer; // used by idlers to flash indicator LED - uint8_t hard_reset_requested; // flag to perform a hard reset - uint8_t bootloader_requested; // flag to enter the bootloader - uint8_t shared_buf_overrun; // flag for shared string buffer overrun condition + // system state variables + uint8_t led_state; // LEGACY // 0=off, 1=on + int32_t led_counter; // LEGACY // a convenience for flashing an LED + uint32_t led_timer; // used by idlers to flash indicator LED + uint8_t hard_reset_requested; // flag to perform a hard reset + uint8_t bootloader_requested; // flag to enter the bootloader + uint8_t shared_buf_overrun; // flag for shared string buffer overrun condition -// uint8_t sync_to_time_state; -// uint32_t sync_to_time_time; +// uint8_t sync_to_time_state; +// uint32_t sync_to_time_time; - int32_t job_id[4]; // uuid to identify the job + int32_t job_id[4]; // uuid to identify the job - // controller serial buffers - char_t *bufp; // pointer to primary or secondary in buffer - char_t in_buf[INPUT_BUFFER_LEN]; // primary input buffer - char_t out_buf[OUTPUT_BUFFER_LEN]; // output buffer - char_t saved_buf[SAVED_BUFFER_LEN]; // save the input buffer - magic_t magic_end; + // controller serial buffers + char_t *bufp; // pointer to primary or secondary in buffer + char_t in_buf[INPUT_BUFFER_LEN]; // primary input buffer + char_t out_buf[OUTPUT_BUFFER_LEN]; // output buffer + char_t saved_buf[SAVED_BUFFER_LEN]; // save the input buffer + magic_t magic_end; } controller_t; -extern controller_t cs; // controller state structure +extern controller_t cs; // controller state structure -enum cmControllerState { // manages startup lines - CONTROLLER_INITIALIZING = 0, // controller is initializing - not ready for use - CONTROLLER_NOT_CONNECTED, // controller has not yet detected connection to USB (or other comm channel) - CONTROLLER_CONNECTED, // controller has connected to USB (or other comm channel) - CONTROLLER_STARTUP, // controller is running startup messages and lines - CONTROLLER_READY // controller is active and ready for use +enum cmControllerState { // manages startup lines + CONTROLLER_INITIALIZING = 0, // controller is initializing - not ready for use + CONTROLLER_NOT_CONNECTED, // controller has not yet detected connection to USB (or other comm channel) + CONTROLLER_CONNECTED, // controller has connected to USB (or other comm channel) + CONTROLLER_STARTUP, // controller is running startup messages and lines + CONTROLLER_READY // controller is active and ready for use }; /**** function prototypes ****/ void controller_init(uint8_t std_in, uint8_t std_out, uint8_t std_err); -void controller_init_assertions(void); -stat_t controller_test_assertions(void); -void controller_run(void); -//void controller_reset(void); +void controller_init_assertions(); +stat_t controller_test_assertions(); +void controller_run(); +//void controller_reset(); -void tg_reset_source(void); +void tg_reset_source(); void tg_set_primary_source(uint8_t dev); void tg_set_secondary_source(uint8_t dev); diff --git a/src/cycle_homing.c b/src/cycle_homing.c index ecf20f1..0e601fc 100755 --- a/src/cycle_homing.c +++ b/src/cycle_homing.c @@ -37,44 +37,36 @@ /**** Homing singleton structure ****/ -struct hmHomingSingleton { // persistent homing runtime variables - // 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 - -#ifndef __NEW_SWITCHES - 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 -#else - int8_t homing_switch_axis; // axis of current homing switch, or -1 if none - uint8_t homing_switch_position; // min/max position of current homing switch - int8_t limit_switch_axis; // axis of current limit switch, or -1 if none - uint8_t limit_switch_position; // min/max position of current limit switch -#endif - void (*switch_saved_on_trailing)(struct swSwitch *s); - - 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 - - // per-axis parameters - float direction; // set to 1 for positive (max), -1 for negative (to min); - 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 - - // state saved from gcode model - uint8_t saved_units_mode; // G20,G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_feed_rate_mode; // G93,G94 global setting - float saved_feed_rate; // F setting - float saved_jerk; // saved and restored for each axis homed +struct hmHomingSingleton { // persistent homing runtime variables + // 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 + + 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 + + // per-axis parameters + float direction; // set to 1 for positive (max), -1 for negative (to min); + 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 + + // state saved from gcode model + uint8_t saved_units_mode; // G20,G21 global setting + uint8_t saved_coord_system; // G54 - G59 setting + uint8_t saved_distance_mode; // G90,G91 global setting + uint8_t saved_feed_rate_mode; // G93,G94 global setting + float saved_feed_rate; // F setting + float saved_jerk; // saved and restored for each axis homed }; static struct hmHomingSingleton hm; @@ -98,294 +90,240 @@ static int8_t _get_next_axis(int8_t axis); ***********************************************************************************/ /***************************************************************************** - * cm_homing_cycle_start() - G28.2 homing cycle using limit switches + * 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) + * 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 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 * - * 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. */ -stat_t cm_homing_cycle_start(void) +stat_t cm_homing_cycle_start() { - // save relevant non-axis parameters from Gcode model - hm.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); - hm.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); - hm.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); - hm.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); - hm.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); - - // set working values - cm_set_units_mode(MILLIMETERS); - cm_set_distance_mode(INCREMENTAL_MODE); - cm_set_coord_system(ABSOLUTE_COORDS); // homing is done in machine coordinates - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); - hm.set_coordinates = true; - - hm.axis = -1; // set to retrieve initial axis - hm.func = _homing_axis_start; // bind initial processing function - cm.cycle_state = CYCLE_HOMING; - cm.homing_state = HOMING_NOT_HOMED; - - return (STAT_OK); + // save relevant non-axis parameters from Gcode model + hm.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); + hm.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); + hm.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); + hm.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); + hm.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); + + // set working values + cm_set_units_mode(MILLIMETERS); + cm_set_distance_mode(INCREMENTAL_MODE); + cm_set_coord_system(ABSOLUTE_COORDS); // homing is done in machine coordinates + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); + hm.set_coordinates = true; + + hm.axis = -1; // set to retrieve initial axis + hm.func = _homing_axis_start; // bind initial processing function + cm.cycle_state = CYCLE_HOMING; + cm.homing_state = HOMING_NOT_HOMED; + + return STAT_OK; } -stat_t cm_homing_cycle_start_no_set(void) +stat_t cm_homing_cycle_start_no_set() { - cm_homing_cycle_start(); - hm.set_coordinates = false; // set flag to not update position variables at the end of the cycle - return (STAT_OK); + cm_homing_cycle_start(); + hm.set_coordinates = false; // set flag to not update position variables at the end of the 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 a 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 + * 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 a 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 */ -stat_t cm_homing_callback(void) +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() == true) { 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)) { - hm.func = func; - return (STAT_EAGAIN); + hm.func = func; + return STAT_EAGAIN; } 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 == -1) { // -1 is done - cm.homing_state = HOMING_HOMED; - return (_set_homing_func(_homing_finalize_exit)); - } else if (axis == -2) { // -2 is error - 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 - cm.homed[axis] = false; - - // trap axis mis-configurations - if (fp_ZERO(cm.a[axis].search_velocity)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY)); - if (fp_ZERO(cm.a[axis].latch_velocity)) return (_homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY)); - if (cm.a[axis].latch_backoff < 0) return (_homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF)); - - // calculate and test travel distance - float travel_distance = 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)); - - // determine the switch setup and that config is OK -#ifndef __NEW_SWITCHES - hm.min_mode = get_switch_mode(MIN_SWITCH(axis)); - hm.max_mode = get_switch_mode(MAX_SWITCH(axis)); -#else - hm.min_mode = get_switch_mode(axis, SW_MIN); - hm.max_mode = get_switch_mode(axis, SW_MAX); -#endif - - if ( ((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) { // one or the other must be homing - return (_homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION)); // axis cannot be homed - } - 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 - - // setup parameters homing to the minimum switch - if (hm.min_mode & SW_HOMING_BIT) { -#ifndef __NEW_SWITCHES - 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 -#else - hm.homing_switch_axis = axis; - hm.homing_switch_position = SW_MIN; // the min is the homing switch - hm.limit_switch_axis = axis; - hm.limit_switch_position = SW_MAX; // the max would be the limit switch -#endif - hm.search_travel = -travel_distance; // search travels in negative direction - hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction - hm.zero_backoff = cm.a[axis].zero_backoff; - - // setup parameters for positive travel (homing to the maximum switch) - } else { -#ifndef __NEW_SWITCHES - 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 -#else - hm.homing_switch_axis = axis; - hm.homing_switch_position = SW_MAX; // the max is the homing switch - hm.limit_switch_axis = axis; - hm.limit_switch_position = SW_MIN; // the min would be the limit switch -#endif - hm.search_travel = travel_distance; // search travels in positive direction - hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction - hm.zero_backoff = -cm.a[axis].zero_backoff; - } - - // if homing is disabled for the axis then skip to the next axis -#ifndef __NEW_SWITCHES - uint8_t sw_mode = get_switch_mode(hm.homing_switch); - 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; -#else -// switch_t *s = &sw.s[hm.homing_switch_axis][hm.homing_switch_position]; -// _bind_switch_settings(s); - _bind_switch_settings(&sw.s[hm.homing_switch_axis][hm.homing_switch_position]); - - uint8_t sw_mode = get_switch_mode(hm.homing_switch_axis, hm.homing_switch_position); - 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_axis, hm.limit_switch_position) == SW_MODE_DISABLED) { - hm.limit_switch_axis = -1; - } -#endif - -// 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 - return (_set_homing_func(_homing_axis_clear)); // start the clear + // get the first or next axis + 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); + } else if (axis == -2) { // -2 is error + 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 + cm.homed[axis] = false; + + // trap axis mis-configurations + if (fp_ZERO(cm.a[axis].search_velocity)) return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY); + if (fp_ZERO(cm.a[axis].latch_velocity)) return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY); + if (cm.a[axis].latch_backoff < 0) return _homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF); + + // calculate and test travel distance + float travel_distance = 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); + + // determine the switch setup and that config is OK + hm.min_mode = get_switch_mode(MIN_SWITCH(axis)); + hm.max_mode = get_switch_mode(MAX_SWITCH(axis)); + + if ( ((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0) { // one or the other must be homing + return _homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION); // axis cannot be homed + } + 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 + + // 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.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.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)) + 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; + +// 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 + 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 +static stat_t _homing_axis_clear(int8_t axis) // first clear move { -#ifndef __NEW_SWITCHES - 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); - } -#else - if (read_switch(hm.homing_switch_axis, hm.homing_switch_position) == SW_CLOSED) { - _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity); - } else if (read_switch(hm.limit_switch_axis, hm.limit_switch_position) == SW_CLOSED) { - _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); - } else { // no move needed, so target position is same as current position - hm.target_position = cm_get_absolute_position(MODEL, axis); - } -#endif - return (_set_homing_func(_homing_axis_search)); + 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); + } + return _set_homing_func(_homing_axis_search); } -static stat_t _homing_axis_search(int8_t axis) // start the search +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 - _homing_axis_move(axis, hm.search_travel, hm.search_velocity); - return (_set_homing_func(_homing_axis_latch)); + cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use the homing jerk for search onward + _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 +static stat_t _homing_axis_latch(int8_t axis) // latch to switch open { - // verify assumption that we arrived here because of homing switch closure - // rather than user-initiated feedhold or other disruption -#ifndef __NEW_SWITCHES - if (sw.state[hm.homing_switch] != SW_CLOSED) - return (_set_homing_func(_homing_abort)); -#else - if (read_switch(hm.homing_switch_axis, hm.homing_switch_position) != SW_CLOSED) - return (_set_homing_func(_homing_abort)); -#endif - _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); - return (_set_homing_func(_homing_axis_zero_backoff)); + // 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) + return _set_homing_func(_homing_abort); + _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); + return _set_homing_func(_homing_axis_zero_backoff); } -static stat_t _homing_axis_zero_backoff(int8_t axis) // backoff to zero position +static stat_t _homing_axis_zero_backoff(int8_t axis) // backoff to zero position { - _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity); - return (_set_homing_func(_homing_axis_set_zero)); + _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 +static stat_t _homing_axis_set_zero(int8_t axis) // set zero and finish up { - if (hm.set_coordinates != false) { - cm_set_position(axis, 0); - cm.homed[axis] = true; - } else { + if (hm.set_coordinates != false) { + cm_set_position(axis, 0); + cm.homed[axis] = true; + } 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 - -#ifdef __NEW_SWITCHES - switch_t *s = &sw.s[hm.homing_switch_axis][hm.homing_switch_position]; - s->on_trailing = hm.switch_saved_on_trailing; - _restore_switch_settings(&sw.s[hm.homing_switch_axis][hm.homing_switch_position]); -#endif - return (_set_homing_func(_homing_axis_start)); + cm_set_position(axis, cm_get_work_position(RUNTIME, axis)); + } + cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value + + return _set_homing_func(_homing_axis_start); } 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}; - - vect[axis] = target; - flags[axis] = true; - cm.gm.feed_rate = velocity; - mp_flush_planner(); // don't use cm_request_queue_flush() here - cm_request_cycle_start(); - ritorno(cm_straight_feed(vect, flags)); - return (STAT_EAGAIN); + float vect[] = {0,0,0,0,0,0}; + float flags[] = {false, false, false, false, false, false}; + + vect[axis] = target; + flags[axis] = true; + cm.gm.feed_rate = velocity; + mp_flush_planner(); // don't use cm_request_queue_flush() here + cm_request_cycle_start(); + ritorno(cm_straight_feed(vect, flags)); + return STAT_EAGAIN; } /* @@ -394,13 +332,10 @@ static stat_t _homing_axis_move(int8_t axis, float target, float velocity) static stat_t _homing_abort(int8_t axis) { - cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value -#ifdef __NEW_SWITCHES - _restore_switch_settings(&sw.s[hm.homing_switch_axis][hm.homing_switch_position]); -#endif - _homing_finalize_exit(axis); - sr_request_status_report(SR_TIMED_REQUEST); - return (STAT_HOMING_CYCLE_FAILED); // homing state remains HOMING_NOT_HOMED + cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value + _homing_finalize_exit(axis); + sr_request_status_report(SR_TIMED_REQUEST); + return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED } /* @@ -409,109 +344,109 @@ static stat_t _homing_abort(int8_t axis) static stat_t _homing_error_exit(int8_t axis, stat_t status) { - // Generate the warning message. Since the error exit returns via the homing callback - // - and not the main controller - it requires its own display processing - nv_reset_nv_list(); - - if (axis == -2) { - nv_add_conditional_message((const char_t *)"Homing error - Bad or no axis(es) specified");; - } else { - char message[NV_MESSAGE_LEN]; - sprintf_P(message, PSTR("Homing error - %c axis settings misconfigured"), cm_get_axis_char(axis)); - nv_add_conditional_message((char_t *)message); - } - nv_print_list(STAT_HOMING_CYCLE_FAILED, TEXT_INLINE_VALUES, JSON_RESPONSE_FORMAT); - - _homing_finalize_exit(axis); - return (STAT_HOMING_CYCLE_FAILED); // homing state remains HOMING_NOT_HOMED + // Generate the warning message. Since the error exit returns via the homing callback + // - and not the main controller - it requires its own display processing + nv_reset_nv_list(); + + if (axis == -2) { + nv_add_conditional_message((const char_t *)"Homing error - Bad or no axis(es) specified");; + } else { + char message[NV_MESSAGE_LEN]; + sprintf_P(message, PSTR("Homing error - %c axis settings misconfigured"), cm_get_axis_char(axis)); + nv_add_conditional_message((char_t *)message); + } + nv_print_list(STAT_HOMING_CYCLE_FAILED, TEXT_INLINE_VALUES, JSON_RESPONSE_FORMAT); + + _homing_finalize_exit(axis); + return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED } /* * _homing_finalize_exit() - helper to finalize homing */ -static stat_t _homing_finalize_exit(int8_t axis) // third part of return to home +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. - // don't use cm_request_queue_flush() here - - cm_set_coord_system(hm.saved_coord_system); // restore to work coordinate 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); - cm.gm.feed_rate = hm.saved_feed_rate; - cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); - cm_cycle_end(); - cm.cycle_state = CYCLE_OFF; - return (STAT_OK); + mp_flush_planner(); // should be stopped, but in case of switch closure. + // don't use cm_request_queue_flush() here + + cm_set_coord_system(hm.saved_coord_system); // restore to work coordinate 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); + cm.gm.feed_rate = hm.saved_feed_rate; + cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); + cm_cycle_end(); + cm.cycle_state = CYCLE_OFF; + return STAT_OK; } /* * _get_next_axis() - 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 - 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) { - 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); - } else if (axis == AXIS_X) { - if (fp_TRUE(cm.gf.target[AXIS_Y])) return (AXIS_Y); - if (fp_TRUE(cm.gf.target[AXIS_A])) return (AXIS_A); - } else if (axis == AXIS_Y) { - if (fp_TRUE(cm.gf.target[AXIS_A])) return (AXIS_A); - } - return (-1); // done + if (axis == -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) { + 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; + } else if (axis == AXIS_X) { + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + } else if (axis == AXIS_Y) { + if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A; + } + return -1; // done #else - if (axis == -1) { - if (fp_TRUE(cm.gf.target[AXIS_Z])) return (AXIS_Z); - if (fp_TRUE(cm.gf.target[AXIS_X])) return (AXIS_X); - if (fp_TRUE(cm.gf.target[AXIS_Y])) return (AXIS_Y); - if (fp_TRUE(cm.gf.target[AXIS_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); - return (-2); // error - } else if (axis == 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); - } else if (axis == 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); - } else if (axis == 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); - } else if (axis == AXIS_A) { - if (fp_TRUE(cm.gf.target[AXIS_B])) return (AXIS_B); - if (fp_TRUE(cm.gf.target[AXIS_C])) return (AXIS_C); - } else if (axis == AXIS_B) { - if (fp_TRUE(cm.gf.target[AXIS_C])) return (AXIS_C); - } - return (-1); // done + if (axis == -1) { + if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z; + if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X; + if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y; + if (fp_TRUE(cm.gf.target[AXIS_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; + return -2; // error + } else if (axis == 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; + } else if (axis == 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; + } else if (axis == 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; + } else if (axis == AXIS_A) { + if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B; + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + } else if (axis == AXIS_B) { + if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C; + } + return -1; // done #endif } diff --git a/src/cycle_jogging.c b/src/cycle_jogging.c index 496bd5b..3448e8d 100755 --- a/src/cycle_jogging.c +++ b/src/cycle_jogging.c @@ -34,23 +34,23 @@ /**** Jogging singleton structure ****/ -struct jmJoggingSingleton { // persistent jogging runtime variables - // controls for jogging cycle - int8_t axis; // axis currently being jogged - float dest_pos; // distance relative to start position to travel - float start_pos; - float velocity_start; // initial jog feed - float velocity_max; - - uint8_t (*func)(int8_t axis); // binding for callback function state machine - - // state saved from gcode model - float saved_feed_rate; // F setting - uint8_t saved_units_mode; // G20,G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_feed_rate_mode; // G93,G94 global setting - float saved_jerk; // saved and restored for each axis homed +struct jmJoggingSingleton { // persistent jogging runtime variables + // controls for jogging cycle + int8_t axis; // axis currently being jogged + float dest_pos; // distance relative to start position to travel + float start_pos; + float velocity_start; // initial jog feed + float velocity_max; + + uint8_t (*func)(int8_t axis); // binding for callback function state machine + + // state saved from gcode model + float saved_feed_rate; // F setting + uint8_t saved_units_mode; // G20,G21 global setting + uint8_t saved_coord_system; // G54 - G59 setting + uint8_t saved_distance_mode; // G90,G91 global setting + uint8_t saved_feed_rate_mode; // G93,G94 global setting + float saved_jerk; // saved and restored for each axis homed }; static struct jmJoggingSingleton jog; @@ -63,20 +63,20 @@ static stat_t _jogging_axis_jog(int8_t axis); static stat_t _jogging_finalize_exit(int8_t axis); /***************************************************************************** - * cm_jogging_cycle_start() - jogging cycle using soft limits + * cm_jogging_cycle_start() - jogging cycle using soft limits * */ -/* --- Some further details --- +/* --- Some further details --- * - * Note: When coding a cycle (like this one) you get to perform one queued - * move per entry into the continuation, then you must exit. + * 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. */ static stat_t _set_jogging_func(uint8_t (*func)(int8_t axis)); @@ -86,121 +86,121 @@ static stat_t _jogging_finalize_exit(int8_t axis); stat_t cm_jogging_cycle_start(uint8_t axis) { - // save relevant non-axis parameters from Gcode model - jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); - jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); - jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); - jog.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); - jog.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); - jog.saved_jerk = cm_get_axis_jerk(axis); - - // set working values - cm_set_units_mode(MILLIMETERS); - cm_set_distance_mode(ABSOLUTE_MODE); - cm_set_coord_system(ABSOLUTE_COORDS); // jogging is done in machine coordinates - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); - - jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h for #define - jog.velocity_max = cm.a[axis].velocity_max; - - jog.start_pos = cm_get_absolute_position(RUNTIME, axis); - jog.dest_pos = cm_get_jogging_dest(); - - jog.axis = axis; - jog.func = _jogging_axis_start; // bind initial processing function - - cm.cycle_state = CYCLE_JOG; - return (STAT_OK); + // save relevant non-axis parameters from Gcode model + jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); + jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); + jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); + jog.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); + jog.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); + jog.saved_jerk = cm_get_axis_jerk(axis); + + // set working values + cm_set_units_mode(MILLIMETERS); + cm_set_distance_mode(ABSOLUTE_MODE); + cm_set_coord_system(ABSOLUTE_COORDS); // jogging is done in machine coordinates + cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); + + jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h for #define + jog.velocity_max = cm.a[axis].velocity_max; + + jog.start_pos = cm_get_absolute_position(RUNTIME, axis); + jog.dest_pos = cm_get_jogging_dest(); + + jog.axis = axis; + jog.func = _jogging_axis_start; // bind initial processing function + + cm.cycle_state = CYCLE_JOG; + return STAT_OK; } /* Jogging axis moves - these execute in sequence for each axis - * cm_jogging_callback() - main loop callback for running the jogging cycle - * _set_jogging_func() - a convenience for setting the next dispatch vector and exiting - * _jogging_axis_start() - setup and start - * _jogging_axis_jog() - ramp the jog - * _jogging_axis_move() - move - * _jogging_finalize_exit() - back off the cleared limit switch + * cm_jogging_callback() - main loop callback for running the jogging cycle + * _set_jogging_func() - a convenience for setting the next dispatch vector and exiting + * _jogging_axis_start() - setup and start + * _jogging_axis_jog() - ramp the jog + * _jogging_axis_move() - move + * _jogging_finalize_exit() - back off the cleared limit switch */ -stat_t cm_jogging_callback(void) +stat_t cm_jogging_callback() { - if (cm.cycle_state != CYCLE_JOG) { return (STAT_NOOP); } // exit if not in a jogging cycle - if (cm_get_runtime_busy() == true) { return (STAT_EAGAIN); } // sync to planner move ends - return (jog.func(jog.axis)); // execute the current homing move + if (cm.cycle_state != CYCLE_JOG) { return STAT_NOOP; } // exit if not in a jogging cycle + if (cm_get_runtime_busy() == true) { return STAT_EAGAIN; } // sync to planner move ends + return jog.func(jog.axis); // execute the current homing move } static stat_t _set_jogging_func(stat_t (*func)(int8_t axis)) { - jog.func = func; - return (STAT_EAGAIN); + jog.func = func; + return STAT_EAGAIN; } static stat_t _jogging_axis_start(int8_t axis) { - return (_set_jogging_func(_jogging_axis_jog)); // register the callback for the jog move + return _set_jogging_func(_jogging_axis_jog); // register the callback for the jog move } -static stat_t _jogging_axis_jog(int8_t axis) // run the jog move +static stat_t _jogging_axis_jog(int8_t axis) // run the jog move { - float vect[] = {0,0,0,0,0,0}; - float flags[] = {false, false, false, false, false, false}; - flags[axis] = true; + float vect[] = {0,0,0,0,0,0}; + float flags[] = {false, false, false, false, false, false}; + flags[axis] = true; - float velocity = jog.velocity_start; - float direction = jog.start_pos <= jog.dest_pos ? 1. : -1.; - float delta = abs(jog.dest_pos - jog.start_pos); + float velocity = jog.velocity_start; + float direction = jog.start_pos <= jog.dest_pos ? 1. : -1.; + float delta = abs(jog.dest_pos - jog.start_pos); - cm.gm.feed_rate = velocity; - mp_flush_planner(); // don't use cm_request_queue_flush() here - cm_request_cycle_start(); + cm.gm.feed_rate = velocity; + mp_flush_planner(); // don't use cm_request_queue_flush() here + cm_request_cycle_start(); #if 1 - float ramp_dist = 2.0; - float steps = 0.0; - float max_steps = 25; - float offset = 0.01; - while( delta>ramp_dist && offset < delta && steps < max_steps ) - { - vect[axis] = jog.start_pos + offset * direction; - cm.gm.feed_rate = velocity; - ritorno(cm_straight_feed(vect, flags)); - - steps++; - float scale = pow(10.0, steps/max_steps) / 10.0; - velocity = jog.velocity_start + (jog.velocity_max - jog.velocity_start) * scale; - offset += ramp_dist * steps/max_steps; - } + float ramp_dist = 2.0; + float steps = 0.0; + float max_steps = 25; + float offset = 0.01; + while( delta>ramp_dist && offset < delta && steps < max_steps ) + { + vect[axis] = jog.start_pos + offset * direction; + cm.gm.feed_rate = velocity; + ritorno(cm_straight_feed(vect, flags)); + + steps++; + float scale = pow(10.0, steps/max_steps) / 10.0; + velocity = jog.velocity_start + (jog.velocity_max - jog.velocity_start) * scale; + offset += ramp_dist * steps/max_steps; + } #else // use a really slow jerk so we ramp up speed // FIXME: need asymmetric accel/deaccel jerk for this to work... -// cm.a[axis].jerk_max = 25; - cm_set_axis_jerk(axis, 25); +// cm.a[axis].jerk_max = 25; + cm_set_axis_jerk(axis, 25); //cm.a[axis].jerk_accel = 10; //cm.a[axis].jerk_deaccel = 900; #endif - // final move - cm.gm.feed_rate = jog.velocity_max; - vect[axis] = jog.dest_pos; - ritorno(cm_straight_feed(vect, flags)); - return (_set_jogging_func(_jogging_finalize_exit)); + // final move + cm.gm.feed_rate = jog.velocity_max; + vect[axis] = jog.dest_pos; + ritorno(cm_straight_feed(vect, flags)); + return _set_jogging_func(_jogging_finalize_exit); } -static stat_t _jogging_finalize_exit(int8_t axis) // finish a jog +static stat_t _jogging_finalize_exit(int8_t axis) // finish a jog { - mp_flush_planner(); // FIXME: not sure what to do on exit - cm_set_axis_jerk(axis, jog.saved_jerk); - cm_set_coord_system(jog.saved_coord_system); // restore to work coordinate system - cm_set_units_mode(jog.saved_units_mode); - cm_set_distance_mode(jog.saved_distance_mode); - cm_set_feed_rate_mode(jog.saved_feed_rate_mode); - cm.gm.feed_rate = jog.saved_feed_rate; - cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); - cm_cycle_end(); - cm.cycle_state = CYCLE_OFF; - - printf("{\"jog\":0}\n"); - return (STAT_OK); + mp_flush_planner(); // FIXME: not sure what to do on exit + cm_set_axis_jerk(axis, jog.saved_jerk); + cm_set_coord_system(jog.saved_coord_system); // restore to work coordinate system + cm_set_units_mode(jog.saved_units_mode); + cm_set_distance_mode(jog.saved_distance_mode); + cm_set_feed_rate_mode(jog.saved_feed_rate_mode); + cm.gm.feed_rate = jog.saved_feed_rate; + cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); + cm_cycle_end(); + cm.cycle_state = CYCLE_OFF; + + printf("{\"jog\":0}\n"); + return STAT_OK; } diff --git a/src/cycle_probing.c b/src/cycle_probing.c index ee2dc40..ddc7ed8 100755 --- a/src/cycle_probing.c +++ b/src/cycle_probing.c @@ -39,30 +39,23 @@ #define MINIMUM_PROBE_TRAVEL 0.254 -struct pbProbingSingleton { // persistent probing runtime variables - stat_t (*func)(); // binding for callback function state machine - - // switch configuration -#ifndef __NEW_SWITCHES - 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 -#else - uint8_t probe_switch_axis; // which axis should we check? - uint8_t probe_switch_position; //...and position - uint8_t saved_switch_type; // saved switch type NO/NC - uint8_t saved_switch_mode; // save the probe switch's original settings -#endif - - // 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 - - // probe destination - float start_position[AXES]; - float target[AXES]; - float flags[AXES]; +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 + + // 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 + + // probe destination + float start_position[AXES]; + float target[AXES]; + float flags[AXES]; }; static struct pbProbingSingleton pb; @@ -81,130 +74,118 @@ static stat_t _probing_error_exit(int8_t axis); uint8_t _set_pb_func(uint8_t (*func)()) { - pb.func = func; - return (STAT_EAGAIN); + 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 + * 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 --- + * --- 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[]) { - // trap zero feed rate condition - 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])) - return (STAT_GCODE_AXIS_IS_MISSING); - - // 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. - // 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 - pb.func = _probing_init; // bind probing initialization function - return (STAT_OK); + // trap zero feed rate condition + 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])) + return STAT_GCODE_AXIS_IS_MISSING; + + // 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. + // 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 + pb.func = _probing_init; // bind probing initialization function + return STAT_OK; } -uint8_t cm_probe_callback(void) +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 + 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 } /* - * _probing_init() - G38.2 homing cycle using limit switches + * _probing_init() - 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() { - // 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. - 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 1 hour the errors can accumulate and send - * results off by as much as a millimeter if not corrected. - * - * Note: Going to doubles (from floats) would reduce the errors but not eliminate - * them altogether. But this moot on AVRGCC which only does single precision floats. - * - * *** Applying the error term for error correction *** - * - * So if you want to use the error from moveA to correct moveB it has to be done in a region that - * is not already running (i.e. the head, body, or tail) as moveB is already 2 segments into run. - * Since most moves in very short line Gcode files are body only, for practical purposes the - * correction will be applied to moveC. (It's possible to recompute the body of moveB, but it may - * not be worth the trouble). + * The purpose of this module is to calculate an error term between the programmed + * position (target) and the actual measured position (position). The error term is + * used during move execution (exec) to adjust the move to compensate for accumulated + * positional errors. It's also the basis of closed-loop (servoed) systems. + * + * Positional error occurs due to floating point numerical inaccuracies. TinyG uses + * 32 bit floating point (GCC 32 bit, which is NOT IEEE 32 bit). Errors creep in + * during planning, move execution, and stepper output phases. Care has been taken + * to minimize introducing errors throughout the process, but they still occur. + * In most cases errors are not noticeable as they fall below the step resolution + * for most jobs. For jobs that run > 1 hour the errors can accumulate and send + * results off by as much as a millimeter if not corrected. + * + * Note: Going to doubles (from floats) would reduce the errors but not eliminate + * them altogether. But this moot on AVRGCC which only does single precision floats. + * + * *** Applying the error term for error correction *** + * + * So if you want to use the error from moveA to correct moveB it has to be done in a region that + * is not already running (i.e. the head, body, or tail) as moveB is already 2 segments into run. + * Since most moves in very short line Gcode files are body only, for practical purposes the + * correction will be applied to moveC. (It's possible to recompute the body of moveB, but it may + * not be worth the trouble). */ #ifndef ENCODER_H_ONCE #define ENCODER_H_ONCE @@ -93,22 +93,22 @@ /**** Macros ****/ // used to abstract the encoder code out of the stepper so it can be managed in one place -#define SET_ENCODER_STEP_SIGN(m,s) en.en[m].step_sign = s; -#define INCREMENT_ENCODER(m) en.en[m].steps_run += en.en[m].step_sign; -#define ACCUMULATE_ENCODER(m) en.en[m].encoder_steps += en.en[m].steps_run; en.en[m].steps_run = 0; +#define SET_ENCODER_STEP_SIGN(m,s) en.en[m].step_sign = s; +#define INCREMENT_ENCODER(m) en.en[m].steps_run += en.en[m].step_sign; +#define ACCUMULATE_ENCODER(m) en.en[m].encoder_steps += en.en[m].steps_run; en.en[m].steps_run = 0; /**** Structures ****/ -typedef struct enEncoder { // one real or virtual encoder per controlled motor - int8_t step_sign; // set to +1 or -1 - int16_t steps_run; // steps counted during stepper interrupt - int32_t encoder_steps; // counted encoder position in steps +typedef struct enEncoder { // one real or virtual encoder per controlled motor + int8_t step_sign; // set to +1 or -1 + int16_t steps_run; // steps counted during stepper interrupt + int32_t encoder_steps; // counted encoder position in steps } enEncoder_t; typedef struct enEncoders { - magic_t magic_start; - enEncoder_t en[MOTORS]; // runtime encoder structures - magic_t magic_end; + magic_t magic_start; + enEncoder_t en[MOTORS]; // runtime encoder structures + magic_t magic_end; } enEncoders_t; extern enEncoders_t en; @@ -116,11 +116,11 @@ extern enEncoders_t en; /**** FUNCTION PROTOTYPES ****/ -void encoder_init(void); -void encoder_init_assertions(void); -stat_t encoder_test_assertions(void); +void encoder_init(); +void encoder_init_assertions(); +stat_t encoder_test_assertions(); void en_set_encoder_steps(uint8_t motor, float steps); float en_read_encoder(uint8_t motor); -#endif // End of include guard: ENCODER_H_ONCE +#endif // End of include guard: ENCODER_H_ONCE diff --git a/src/gcode/gcode_circles2.h b/src/gcode/gcode_circles2.h index a2d7811..9fa7a36 100755 --- a/src/gcode/gcode_circles2.h +++ b/src/gcode/gcode_circles2.h @@ -4,7 +4,7 @@ // circles test #2 const char PROGMEM gcode_file[] = "\ -G20 (inches mode)\n\ +G20 (inches mode)\n\ G40 G17\n\ T1 M06\n\ M3\n\ diff --git a/src/gcode/gcode_debug_tests.h b/src/gcode/gcode_debug_tests.h index 4a00da7..99572ad 100755 --- a/src/gcode/gcode_debug_tests.h +++ b/src/gcode/gcode_debug_tests.h @@ -7,13 +7,13 @@ N3 G92X-11.000Y-3.000Z0.000\n\ N4 F600.0\n\ N500 G1X-11.000Y-3.000\n\ N545 X-11.440Y-3.415\n\ -N546 X-11.491Y-3.472 (***HT!***)\n\ -N547 X-11.556Y-3.532 (***HT!***)\n\ -N548 X-11.636Y-3.594 (***HT!***)\n\ +N546 X-11.491Y-3.472 (***HT!***)\n\ +N547 X-11.556Y-3.532 (***HT!***)\n\ +N548 X-11.636Y-3.594 (***HT!***)\n\ N549 X-11.729Y-3.658\n\ -N550 X-11.837Y-3.724 (***HT!***)\n\ +N550 X-11.837Y-3.724 (***HT!***)\n\ N551 X-11.959Y-3.793\n\ -N552 X-12.094Y-3.863 (***HT!***)\n\ +N552 X-12.094Y-3.863 (***HT!***)\n\ N553 X-12.242Y-3.936\n\ N554 X-12.404Y-4.010\n\ N555 X-12.767Y-4.165\n\ @@ -29,7 +29,7 @@ N564 X-20.146Y-6.431\n\ N565 X-20.516Y-6.532\n\ N566 X-20.887Y-6.634\n\ N567 X-21.630Y-6.837\n\ -N568 X-23.106Y-7.241 (***Steppers!***)\n\ +N568 X-23.106Y-7.241 (***Steppers!***)\n\ N569 X-25.895Y-8.028\n\ N570 X-26.218Y-8.123\n\ N571 X-26.535Y-8.218"; diff --git a/src/gcode/gcode_tests.h b/src/gcode/gcode_tests.h index 696cdae..51e8f5c 100755 --- a/src/gcode/gcode_tests.h +++ b/src/gcode/gcode_tests.h @@ -340,7 +340,7 @@ g2 f800 x10 y10 i5 j5\n\ g3 f800 x0 y0 i5 j5\n\ g0 x0 y0 z0"; */ -// G1 F600 x21.45 y76.0982\n\ // test max feed rate error +// G1 F600 x21.45 y76.0982\n\ // test max feed rate error const char PROGMEM square_test1[] = "\ g1 f333 x1 y0\n\ diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 31be692..2ebf8f4 100755 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -16,26 +16,26 @@ * 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 "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "controller.h" #include "gcode_parser.h" #include "canonical_machine.h" #include "spindle.h" #include "util.h" -#include "xio.h" // for char definitions +#include "xio/xio.h" // for char definitions -struct gcodeParserSingleton { // struct to manage globals - uint8_t modals[MODAL_GROUP_COUNT];// collects modal groups in a block +struct gcodeParserSingleton { // struct to manage globals + uint8_t modals[MODAL_GROUP_COUNT];// collects modal groups in a block }; struct gcodeParserSingleton gp; // local helper functions and macros static void _normalize_gcode_block(char_t *str, char_t **com, char_t **msg, uint8_t *block_delete_flag); static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value); static stat_t _point(float value); -static stat_t _validate_gcode_block(void); -static stat_t _parse_gcode_block(char_t *line); // Parse the block into the GN/GF structs -static stat_t _execute_gcode_block(void); // Execute the gcode block +static stat_t _validate_gcode_block(); +static stat_t _parse_gcode_block(char_t *line); // Parse the block into the GN/GF structs +static stat_t _execute_gcode_block(); // Execute the gcode block #define SET_MODAL(m,parm,val) ({cm.gn.parm=val; cm.gf.parm=1; gp.modals[m]+=1; break;}) #define SET_NON_MODAL(parm,val) ({cm.gn.parm=val; cm.gf.parm=1; break;}) @@ -44,152 +44,147 @@ static stat_t _execute_gcode_block(void); // Execute the gcode block /* * gc_gcode_parser() - parse a block (line) of gcode * - * Top level of gcode parser. Normalizes block and looks for special cases + * Top level of gcode parser. Normalizes block and looks for special cases */ stat_t gc_gcode_parser(char_t *block) { - char_t *str = block; // gcode command or NUL string - char_t none = NUL; - char_t *com = &none; // gcode comment or NUL string - char_t *msg = &none; // gcode message or NUL string - uint8_t block_delete_flag; - - // don't process Gcode blocks if in alarmed state - if (cm.machine_state == MACHINE_ALARM) return (STAT_MACHINE_ALARMED); - - _normalize_gcode_block(str, &com, &msg, &block_delete_flag); - - // Block delete omits the line if a / char is present in the first space - // For now this is unconditional and will always delete -// if ((block_delete_flag == true) && (cm_get_block_delete_switch() == true)) { - if (block_delete_flag == true) { - return (STAT_NOOP); - } - - // queue a "(MSG" response - if (*msg != NUL) { - (void)cm_message(msg); // queue the message - } - - return(_parse_gcode_block(block)); + char_t *str = block; // gcode command or 0 string + char_t none = 0; + char_t *com = &none; // gcode comment or 0 string + char_t *msg = &none; // gcode message or 0 string + uint8_t block_delete_flag; + + // don't process Gcode blocks if in alarmed state + if (cm.machine_state == MACHINE_ALARM) return STAT_MACHINE_ALARMED; + + _normalize_gcode_block(str, &com, &msg, &block_delete_flag); + + // Block delete omits the line if a / char is present in the first space + // For now this is unconditional and will always delete + if (block_delete_flag) return STAT_NOOP; + + // queue a "(MSG" response + if (*msg) cm_message(msg); // queue the message + + return(_parse_gcode_block(block)); } /* * _normalize_gcode_block() - normalize a block (line) of gcode in place * - * Normalization functions: + * Normalization functions: * - convert all letters to upper case - * - remove white space, control and other invalid characters - * - remove (erroneous) leading zeros that might be taken to mean Octal - * - identify and return start of comments and messages - * - signal if a block-delete character (/) was encountered in the first space + * - remove white space, control and other invalid characters + * - remove (erroneous) leading zeros that might be taken to mean Octal + * - identify and return start of comments and messages + * - signal if a block-delete character (/) was encountered in the first space * - * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400" + * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400" * - * Comment and message handling: - * - Comments field start with a '(' char or alternately a semicolon ';' - * - Comments and messages are not normalized - they are left alone - * - The 'MSG' specifier in comment can have mixed case but cannot cannot have embedded white spaces - * - Normalization returns true if there was a message to display, false otherwise - * - Comments always terminate the block - i.e. leading or embedded comments are not supported - * - Valid cases (examples) Notes: - * G0X10 - command only - no comment - * (comment text) - There is no command on this line - * G0X10 (comment text) - * G0X10 (comment text - It's OK to drop the trailing paren - * G0X10 ;comment text - It's OK to drop the trailing paren + * Comment and message handling: + * - Comments field start with a '(' char or alternately a semicolon ';' + * - Comments and messages are not normalized - they are left alone + * - The 'MSG' specifier in comment can have mixed case but cannot cannot have embedded white spaces + * - Normalization returns true if there was a message to display, false otherwise + * - Comments always terminate the block - i.e. leading or embedded comments are not supported + * - Valid cases (examples) Notes: + * G0X10 - command only - no comment + * (comment text) - There is no command on this line + * G0X10 (comment text) + * G0X10 (comment text - It's OK to drop the trailing paren + * G0X10 ;comment text - It's OK to drop the trailing paren * - * - Invalid cases (examples) Notes: - * G0X10 comment text - Comment with no separator - * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored - * (comment) G0X10 - leading comment. G0X10 will be ignored - * G0X10 # comment - invalid separator + * - Invalid cases (examples) Notes: + * G0X10 comment text - Comment with no separator + * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored + * (comment) G0X10 - leading comment. G0X10 will be ignored + * G0X10 # comment - invalid separator * - * Returns: - * - com points to comment string or to NUL if no comment - * - msg points to message string or to NUL if no comment - * - block_delete_flag is set true if block delete encountered, false otherwise + * Returns: + * - com points to comment string or to 0 if no comment + * - msg points to message string or to 0 if no comment + * - block_delete_flag is set true if block delete encountered, false otherwise */ static void _normalize_gcode_block(char_t *str, char_t **com, char_t **msg, uint8_t *block_delete_flag) { - char_t *rd = str; // read pointer - char_t *wr = str; // write pointer - - // Preset comments and messages to NUL string - // Not required if com and msg already point to NUL on entry -// for (rd = str; *rd != NUL; rd++) { if (*rd == NUL) { *com = rd; *msg = rd; rd = str;} } - - // mark block deletes - if (*rd == '/') { *block_delete_flag = true; } - else { *block_delete_flag = false; } - - // normalize the command block & find the comment (if any) - for (; *wr != NUL; rd++) { - if (*rd == NUL) { *wr = NUL; } - else if ((*rd == '(') || (*rd == ';')) { *wr = NUL; *com = rd+1; } - else if ((isalnum((char)*rd)) || (strchr("-.", *rd))) { // all valid characters - *(wr++) = (char_t)toupper((char)*(rd)); - } - } - - // Perform Octal stripping - remove invalid leading zeros in number strings - rd = str; - while (*rd != NUL) { - if (*rd == '.') break; // don't strip past a decimal point - if ((!isdigit(*rd)) && (*(rd+1) == '0') && (isdigit(*(rd+2)))) { - wr = rd+1; - while (*wr != NUL) { *wr = *(wr+1); wr++;} // copy forward w/overwrite - continue; - } - rd++; - } - - // process comments and messages - if (**com != NUL) { - rd = *com; - while (isspace(*rd)) { rd++; } // skip any leading spaces before "msg" - if ((tolower(*rd) == 'm') && (tolower(*(rd+1)) == 's') && (tolower(*(rd+2)) == 'g')) { - *msg = rd+3; - } - for (; *rd != NUL; rd++) { - if (*rd == ')') *rd = NUL; // NUL terminate on trailing parenthesis, if any - } - } + char_t *rd = str; // read pointer + char_t *wr = str; // write pointer + + // Preset comments and messages to 0 string + // Not required if com and msg already point to 0 on entry +// for (rd = str; *rd != 0; rd++) { if (*rd == 0) { *com = rd; *msg = rd; rd = str;} } + + // mark block deletes + if (*rd == '/') { *block_delete_flag = true; } + else { *block_delete_flag = false; } + + // normalize the command block & find the comment (if any) + for (; *wr != 0; rd++) { + if (*rd == 0) { *wr = 0; } + else if ((*rd == '(') || (*rd == ';')) { *wr = 0; *com = rd+1; } + else if ((isalnum((char)*rd)) || (strchr("-.", *rd))) { // all valid characters + *(wr++) = (char_t)toupper((char)*(rd)); + } + } + + // Perform Octal stripping - remove invalid leading zeros in number strings + rd = str; + while (*rd != 0) { + if (*rd == '.') break; // don't strip past a decimal point + if ((!isdigit(*rd)) && (*(rd+1) == '0') && (isdigit(*(rd+2)))) { + wr = rd+1; + while (*wr != 0) { *wr = *(wr+1); wr++;} // copy forward w/overwrite + continue; + } + rd++; + } + + // process comments and messages + if (**com != 0) { + rd = *com; + while (isspace(*rd)) { rd++; } // skip any leading spaces before "msg" + if ((tolower(*rd) == 'm') && (tolower(*(rd+1)) == 's') && (tolower(*(rd+2)) == 'g')) { + *msg = rd+3; + } + for (; *rd != 0; rd++) { + if (*rd == ')') *rd = 0; // 0 terminate on trailing parenthesis, if any + } + } } /* * _get_next_gcode_word() - get gcode word consisting of a letter and a value * - * This function requires the Gcode string to be normalized. - * Normalization must remove any leading zeros or they will be converted to Octal - * G0X... is not interpreted as hexadecimal. This is trapped. + * This function requires the Gcode string to be normalized. + * Normalization must remove any leading zeros or they will be converted to Octal + * G0X... is not interpreted as hexadecimal. This is trapped. */ static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { - if (**pstr == NUL) - return (STAT_COMPLETE); // no more words to process - - // get letter part - if(isupper(**pstr) == false) - return (STAT_INVALID_OR_MALFORMED_COMMAND); - *letter = **pstr; - (*pstr)++; - - // X-axis-becomes-a-hexadecimal-number get-value case, e.g. G0X100 --> G255 - if ((**pstr == '0') && (*(*pstr+1) == 'X')) { - *value = 0; - (*pstr)++; - return (STAT_OK); // pointer points to X - } - - // get-value general case - char *end; - *value = strtof(*pstr, &end); - if(end == *pstr) + if (**pstr == 0) + return STAT_COMPLETE; // no more words to process + + // get letter part + if(isupper(**pstr) == false) + return STAT_INVALID_OR_MALFORMED_COMMAND; + *letter = **pstr; + (*pstr)++; + + // X-axis-becomes-a-hexadecimal-number get-value case, e.g. G0X100 --> G255 + if ((**pstr == '0') && (*(*pstr+1) == 'X')) { + *value = 0; + (*pstr)++; + return STAT_OK; // pointer points to X + } + + // get-value general case + char *end; + *value = strtof(*pstr, &end); + if(end == *pstr) return(STAT_BAD_NUMBER_FORMAT); // more robust test then checking for value=0; - *pstr = end; - return (STAT_OK); // pointer points to next character after the word + *pstr = end; + return STAT_OK; // pointer points to next character after the word } /* @@ -197,7 +192,7 @@ static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) */ static uint8_t _point(float value) { - return((uint8_t)(value*10 - trunc(value)*10)); // isolate the decimal point as an int + return((uint8_t)(value*10 - trunc(value)*10)); // isolate the decimal point as an int } /* @@ -206,307 +201,307 @@ static uint8_t _point(float value) static stat_t _validate_gcode_block() { - // Check for modal group violations. From NIST, section 3.4 "It is an error to put - // a G-code from group 1 and a G-code from group 0 on the same line if both of them - // use axis words. If an axis word-using G-code from group 1 is implicitly in effect - // on a line (by having been activated on an earlier line), and a group 0 G-code that - // uses axis words appears on the line, the activity of the group 1 G-code is suspended - // for that line. The axis word-using G-codes from group 0 are G10, G28, G30, and G92" - -// if ((gp.modals[MODAL_GROUP_G0] == true) && (gp.modals[MODAL_GROUP_G1] == true)) { -// return (STAT_MODAL_GROUP_VIOLATION); -// } - - // look for commands that require an axis word to be present -// if ((gp.modals[MODAL_GROUP_G0] == true) || (gp.modals[MODAL_GROUP_G1] == true)) { -// if (_axis_changed() == false) -// return (STAT_GCODE_AXIS_IS_MISSING); -// } - return (STAT_OK); + // Check for modal group violations. From NIST, section 3.4 "It is an error to put + // a G-code from group 1 and a G-code from group 0 on the same line if both of them + // use axis words. If an axis word-using G-code from group 1 is implicitly in effect + // on a line (by having been activated on an earlier line), and a group 0 G-code that + // uses axis words appears on the line, the activity of the group 1 G-code is suspended + // for that line. The axis word-using G-codes from group 0 are G10, G28, G30, and G92" + +// if ((gp.modals[MODAL_GROUP_G0] == true) && (gp.modals[MODAL_GROUP_G1] == true)) { +// return STAT_MODAL_GROUP_VIOLATION; +// } + + // look for commands that require an axis word to be present +// if ((gp.modals[MODAL_GROUP_G0] == true) || (gp.modals[MODAL_GROUP_G1] == true)) { +// if (_axis_changed() == false) +// return STAT_GCODE_AXIS_IS_MISSING; +// } + return STAT_OK; } /* - * _parse_gcode_block() - parses one line of NULL terminated G-Code. + * _parse_gcode_block() - parses one line of 0 terminated G-Code. * - * All the parser does is load the state values in gn (next model state) and set flags - * in gf (model state flags). The execute routine applies them. The buffer is assumed to - * contain only uppercase characters and signed floats (no whitespace). + * All the parser does is load the state values in gn (next model state) and set flags + * in gf (model state flags). The execute routine applies them. The buffer is assumed to + * contain only uppercase characters and signed floats (no whitespace). * - * A number of implicit things happen when the gn struct is zeroed: - * - inverse feed rate mode is canceled - set back to units_per_minute mode + * A number of implicit things happen when the gn struct is zeroed: + * - inverse feed rate mode is canceled - set back to units_per_minute mode */ static stat_t _parse_gcode_block(char_t *buf) { - char *pstr = (char *)buf; // persistent pointer into gcode block for parsing words - char letter; // parsed letter, eg.g. G or X or Y - float value = 0; // value parsed from letter (e.g. 2 for G2) - stat_t status = STAT_OK; - - // set initial state for new move - memset(&gp, 0, sizeof(gp)); // clear all parser values - memset(&cm.gf, 0, sizeof(GCodeInput_t)); // clear all next-state flags - memset(&cm.gn, 0, sizeof(GCodeInput_t)); // clear all next-state values - cm.gn.motion_mode = cm_get_motion_mode(MODEL); // get motion mode from previous block - - // extract commands and parameters - while((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { - switch(letter) { - case 'G': - switch((uint8_t)value) { - case 0: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE); - case 1: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED); - case 2: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC); - case 3: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC); - case 4: SET_NON_MODAL (next_action, NEXT_ACTION_DWELL); - case 10: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA); - case 17: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XY); - case 18: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XZ); - case 19: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_YZ); - case 20: SET_MODAL (MODAL_GROUP_G6, units_mode, INCHES); - case 21: SET_MODAL (MODAL_GROUP_G6, units_mode, MILLIMETERS); - case 28: { - switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); - case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); - case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SEARCH_HOME); - case 3: SET_NON_MODAL (next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN); - case 4: SET_NON_MODAL (next_action, NEXT_ACTION_HOMING_NO_SET); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - } - case 30: { - switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); - case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - } - case 38: { - switch (_point(value)) { - case 2: SET_NON_MODAL (next_action, NEXT_ACTION_STRAIGHT_PROBE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - } - case 40: break; // ignore cancel cutter radius compensation - case 49: break; // ignore cancel tool length offset comp. - case 53: SET_NON_MODAL (absolute_override, true); - case 54: SET_MODAL (MODAL_GROUP_G12, coord_system, G54); - case 55: SET_MODAL (MODAL_GROUP_G12, coord_system, G55); - case 56: SET_MODAL (MODAL_GROUP_G12, coord_system, G56); - case 57: SET_MODAL (MODAL_GROUP_G12, coord_system, G57); - case 58: SET_MODAL (MODAL_GROUP_G12, coord_system, G58); - case 59: SET_MODAL (MODAL_GROUP_G12, coord_system, G59); - case 61: { - switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_PATH); - case 1: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_STOP); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - } - case 64: SET_MODAL (MODAL_GROUP_G13,path_control, PATH_CONTINUOUS); - case 80: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CANCEL_MOTION_MODE); -// case 90: SET_MODAL (MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); -// case 91: SET_MODAL (MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); - case 90: { - switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); - case 1: SET_MODAL (MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - } - case 91: { - switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); - case 1: SET_MODAL (MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - } - case 92: { - switch (_point(value)) { - case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_ORIGIN_OFFSETS); - case 1: SET_NON_MODAL (next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS); - case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS); - case 3: SET_NON_MODAL (next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - } - case 93: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, INVERSE_TIME_MODE); - case 94: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_MINUTE_MODE); -// case 95: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_REVOLUTION_MODE); - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 'M': - switch((uint8_t)value) { - case 0: case 1: case 60: - SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_STOP); - case 2: case 30: - SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_END); - case 3: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CW); - case 4: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW); - case 5: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF); - case 6: SET_NON_MODAL (tool_change, true); - case 7: SET_MODAL (MODAL_GROUP_M8, mist_coolant, true); - case 8: SET_MODAL (MODAL_GROUP_M8, flood_coolant, true); - case 9: SET_MODAL (MODAL_GROUP_M8, flood_coolant, false); - case 48: SET_MODAL (MODAL_GROUP_M9, override_enables, true); - case 49: SET_MODAL (MODAL_GROUP_M9, override_enables, false); - case 50: SET_MODAL (MODAL_GROUP_M9, feed_rate_override_enable, true); // conditionally true - case 51: SET_MODAL (MODAL_GROUP_M9, spindle_override_enable, true); // conditionally true - default: status = STAT_MCODE_COMMAND_UNSUPPORTED; - } - break; - - case 'T': SET_NON_MODAL (tool_select, (uint8_t)trunc(value)); - case 'F': SET_NON_MODAL (feed_rate, value); - case 'P': SET_NON_MODAL (parameter, value); // used for dwell time, G10 coord select, rotations - case 'S': SET_NON_MODAL (spindle_speed, value); - case 'X': SET_NON_MODAL (target[AXIS_X], value); - case 'Y': SET_NON_MODAL (target[AXIS_Y], value); - case 'Z': SET_NON_MODAL (target[AXIS_Z], value); - case 'A': SET_NON_MODAL (target[AXIS_A], value); - case 'B': SET_NON_MODAL (target[AXIS_B], value); - case 'C': SET_NON_MODAL (target[AXIS_C], value); - // case 'U': SET_NON_MODAL (target[AXIS_U], value); // reserved - // case 'V': SET_NON_MODAL (target[AXIS_V], value); // reserved - // case 'W': SET_NON_MODAL (target[AXIS_W], value); // reserved - case 'I': SET_NON_MODAL (arc_offset[0], value); - case 'J': SET_NON_MODAL (arc_offset[1], value); - case 'K': SET_NON_MODAL (arc_offset[2], value); - case 'R': SET_NON_MODAL (arc_radius, value); - case 'N': SET_NON_MODAL (linenum,(uint32_t)value); // line number - case 'L': break; // not used for anything - default: status = STAT_GCODE_COMMAND_UNSUPPORTED; - } - if(status != STAT_OK) break; - } - if ((status != STAT_OK) && (status != STAT_COMPLETE)) return (status); - ritorno(_validate_gcode_block()); - return (_execute_gcode_block()); // if successful execute the block + char *pstr = (char *)buf; // persistent pointer into gcode block for parsing words + char letter; // parsed letter, eg.g. G or X or Y + float value = 0; // value parsed from letter (e.g. 2 for G2) + stat_t status = STAT_OK; + + // set initial state for new move + memset(&gp, 0, sizeof(gp)); // clear all parser values + memset(&cm.gf, 0, sizeof(GCodeInput_t)); // clear all next-state flags + memset(&cm.gn, 0, sizeof(GCodeInput_t)); // clear all next-state values + cm.gn.motion_mode = cm_get_motion_mode(MODEL); // get motion mode from previous block + + // extract commands and parameters + while((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { + switch(letter) { + case 'G': + switch((uint8_t)value) { + case 0: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE); + case 1: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED); + case 2: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC); + case 3: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC); + case 4: SET_NON_MODAL (next_action, NEXT_ACTION_DWELL); + case 10: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA); + case 17: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XY); + case 18: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_XZ); + case 19: SET_MODAL (MODAL_GROUP_G2, select_plane, CANON_PLANE_YZ); + case 20: SET_MODAL (MODAL_GROUP_G6, units_mode, INCHES); + case 21: SET_MODAL (MODAL_GROUP_G6, units_mode, MILLIMETERS); + case 28: { + switch (_point(value)) { + case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); + case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); + case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SEARCH_HOME); + case 3: SET_NON_MODAL (next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN); + case 4: SET_NON_MODAL (next_action, NEXT_ACTION_HOMING_NO_SET); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + } + case 30: { + switch (_point(value)) { + case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); + case 1: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + } + case 38: { + switch (_point(value)) { + case 2: SET_NON_MODAL (next_action, NEXT_ACTION_STRAIGHT_PROBE); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + } + case 40: break; // ignore cancel cutter radius compensation + case 49: break; // ignore cancel tool length offset comp. + case 53: SET_NON_MODAL (absolute_override, true); + case 54: SET_MODAL (MODAL_GROUP_G12, coord_system, G54); + case 55: SET_MODAL (MODAL_GROUP_G12, coord_system, G55); + case 56: SET_MODAL (MODAL_GROUP_G12, coord_system, G56); + case 57: SET_MODAL (MODAL_GROUP_G12, coord_system, G57); + case 58: SET_MODAL (MODAL_GROUP_G12, coord_system, G58); + case 59: SET_MODAL (MODAL_GROUP_G12, coord_system, G59); + case 61: { + switch (_point(value)) { + case 0: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_PATH); + case 1: SET_MODAL (MODAL_GROUP_G13, path_control, PATH_EXACT_STOP); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + } + case 64: SET_MODAL (MODAL_GROUP_G13,path_control, PATH_CONTINUOUS); + case 80: SET_MODAL (MODAL_GROUP_G1, motion_mode, MOTION_MODE_CANCEL_MOTION_MODE); +// case 90: SET_MODAL (MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); +// case 91: SET_MODAL (MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); + case 90: { + switch (_point(value)) { + case 0: SET_MODAL (MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); + case 1: SET_MODAL (MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + } + case 91: { + switch (_point(value)) { + case 0: SET_MODAL (MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); + case 1: SET_MODAL (MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + } + case 92: { + switch (_point(value)) { + case 0: SET_MODAL (MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_ORIGIN_OFFSETS); + case 1: SET_NON_MODAL (next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS); + case 2: SET_NON_MODAL (next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS); + case 3: SET_NON_MODAL (next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + } + case 93: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, INVERSE_TIME_MODE); + case 94: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_MINUTE_MODE); +// case 95: SET_MODAL (MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_REVOLUTION_MODE); + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + break; + + case 'M': + switch((uint8_t)value) { + case 0: case 1: case 60: + SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_STOP); + case 2: case 30: + SET_MODAL (MODAL_GROUP_M4, program_flow, PROGRAM_END); + case 3: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CW); + case 4: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW); + case 5: SET_MODAL (MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF); + case 6: SET_NON_MODAL (tool_change, true); + case 7: SET_MODAL (MODAL_GROUP_M8, mist_coolant, true); + case 8: SET_MODAL (MODAL_GROUP_M8, flood_coolant, true); + case 9: SET_MODAL (MODAL_GROUP_M8, flood_coolant, false); + case 48: SET_MODAL (MODAL_GROUP_M9, override_enables, true); + case 49: SET_MODAL (MODAL_GROUP_M9, override_enables, false); + case 50: SET_MODAL (MODAL_GROUP_M9, feed_rate_override_enable, true); // conditionally true + case 51: SET_MODAL (MODAL_GROUP_M9, spindle_override_enable, true); // conditionally true + default: status = STAT_MCODE_COMMAND_UNSUPPORTED; + } + break; + + case 'T': SET_NON_MODAL (tool_select, (uint8_t)trunc(value)); + case 'F': SET_NON_MODAL (feed_rate, value); + case 'P': SET_NON_MODAL (parameter, value); // used for dwell time, G10 coord select, rotations + case 'S': SET_NON_MODAL (spindle_speed, value); + case 'X': SET_NON_MODAL (target[AXIS_X], value); + case 'Y': SET_NON_MODAL (target[AXIS_Y], value); + case 'Z': SET_NON_MODAL (target[AXIS_Z], value); + case 'A': SET_NON_MODAL (target[AXIS_A], value); + case 'B': SET_NON_MODAL (target[AXIS_B], value); + case 'C': SET_NON_MODAL (target[AXIS_C], value); + // case 'U': SET_NON_MODAL (target[AXIS_U], value); // reserved + // case 'V': SET_NON_MODAL (target[AXIS_V], value); // reserved + // case 'W': SET_NON_MODAL (target[AXIS_W], value); // reserved + case 'I': SET_NON_MODAL (arc_offset[0], value); + case 'J': SET_NON_MODAL (arc_offset[1], value); + case 'K': SET_NON_MODAL (arc_offset[2], value); + case 'R': SET_NON_MODAL (arc_radius, value); + case 'N': SET_NON_MODAL (linenum,(uint32_t)value); // line number + case 'L': break; // not used for anything + default: status = STAT_GCODE_COMMAND_UNSUPPORTED; + } + if(status != STAT_OK) break; + } + if ((status != STAT_OK) && (status != STAT_COMPLETE)) return status; + ritorno(_validate_gcode_block()); + return _execute_gcode_block(); // if successful execute the block } /* * _execute_gcode_block() - execute parsed block * * Conditionally (based on whether a flag is set in gf) call the canonical - * machining functions in order of execution as per RS274NGC_3 table 8 + * machining functions in order of execution as per RS274NGC_3 table 8 * (below, with modifications): * - * 0. record the line number - * 1. comment (includes message) [handled during block normalization] - * 2. set feed rate mode (G93, G94 - inverse time or per minute) - * 3. set feed rate (F) - * 3a. set feed override rate (M50.1) - * 3a. set traverse override rate (M50.2) - * 4. set spindle speed (S) - * 4a. set spindle override rate (M51.1) - * 5. select tool (T) - * 6. change tool (M6) - * 7. spindle on or off (M3, M4, M5) - * 8. coolant on or off (M7, M8, M9) - * 9. enable or disable overrides (M48, M49, M50, M51) - * 10. dwell (G4) - * 11. set active plane (G17, G18, G19) - * 12. set length units (G20, G21) - * 13. cutter radius compensation on or off (G40, G41, G42) - * 14. cutter length compensation on or off (G43, G49) - * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) - * 16. set path control mode (G61, G61.1, G64) - * 17. set distance mode (G90, G91) - * 18. set retract mode (G98, G99) - * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) - * 19b. update system data (G10) - * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) - * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 - * 21. stop and end (M0, M1, M2, M30, M60) + * 0. record the line number + * 1. comment (includes message) [handled during block normalization] + * 2. set feed rate mode (G93, G94 - inverse time or per minute) + * 3. set feed rate (F) + * 3a. set feed override rate (M50.1) + * 3a. set traverse override rate (M50.2) + * 4. set spindle speed (S) + * 4a. set spindle override rate (M51.1) + * 5. select tool (T) + * 6. change tool (M6) + * 7. spindle on or off (M3, M4, M5) + * 8. coolant on or off (M7, M8, M9) + * 9. enable or disable overrides (M48, M49, M50, M51) + * 10. dwell (G4) + * 11. set active plane (G17, G18, G19) + * 12. set length units (G20, G21) + * 13. cutter radius compensation on or off (G40, G41, G42) + * 14. cutter length compensation on or off (G43, G49) + * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) + * 16. set path control mode (G61, G61.1, G64) + * 17. set distance mode (G90, G91) + * 18. set retract mode (G98, G99) + * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) + * 19b. update system data (G10) + * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) + * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 + * 21. stop and end (M0, M1, M2, M30, M60) * - * Values in gn are in original units and should not be unit converted prior - * to calling the canonical functions (which do the unit conversions) + * Values in gn are in original units and should not be unit converted prior + * to calling the canonical functions (which do the unit conversions) */ static stat_t _execute_gcode_block() { - stat_t status = STAT_OK; - - cm_set_model_linenum(cm.gn.linenum); - EXEC_FUNC(cm_set_feed_rate_mode, feed_rate_mode); - EXEC_FUNC(cm_set_feed_rate, feed_rate); - EXEC_FUNC(cm_feed_rate_override_factor, feed_rate_override_factor); - EXEC_FUNC(cm_traverse_override_factor, traverse_override_factor); - EXEC_FUNC(cm_set_spindle_speed, spindle_speed); - EXEC_FUNC(cm_spindle_override_factor, spindle_override_factor); - EXEC_FUNC(cm_select_tool, tool_select); // tool_select is where it's written - EXEC_FUNC(cm_change_tool, tool_change); - EXEC_FUNC(cm_spindle_control, spindle_mode); // spindle on or off - EXEC_FUNC(cm_mist_coolant_control, mist_coolant); - EXEC_FUNC(cm_flood_coolant_control, flood_coolant); // also disables mist coolant if OFF - EXEC_FUNC(cm_feed_rate_override_enable, feed_rate_override_enable); - EXEC_FUNC(cm_traverse_override_enable, traverse_override_enable); - EXEC_FUNC(cm_spindle_override_enable, spindle_override_enable); - EXEC_FUNC(cm_override_enables, override_enables); - - if (cm.gn.next_action == NEXT_ACTION_DWELL) { // G4 - dwell - ritorno(cm_dwell(cm.gn.parameter)); // return if error, otherwise complete the block - } - EXEC_FUNC(cm_select_plane, select_plane); - EXEC_FUNC(cm_set_units_mode, units_mode); - //--> cutter radius compensation goes here - //--> cutter length compensation goes here - EXEC_FUNC(cm_set_coord_system, coord_system); - EXEC_FUNC(cm_set_path_control, path_control); - EXEC_FUNC(cm_set_distance_mode, distance_mode); - //--> set retract mode goes here - - switch (cm.gn.next_action) { - case NEXT_ACTION_SET_G28_POSITION: { status = cm_set_g28_position(); break;} // G28.1 - case NEXT_ACTION_GOTO_G28_POSITION: { status = cm_goto_g28_position(cm.gn.target, cm.gf.target); break;} // G28 - case NEXT_ACTION_SET_G30_POSITION: { status = cm_set_g30_position(); break;} // G30.1 - case NEXT_ACTION_GOTO_G30_POSITION: { status = cm_goto_g30_position(cm.gn.target, cm.gf.target); break;} // G30 - - case NEXT_ACTION_SEARCH_HOME: { status = cm_homing_cycle_start(); break;} // G28.2 - case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: { status = cm_set_absolute_origin(cm.gn.target, cm.gf.target); break;}// G28.3 - case NEXT_ACTION_HOMING_NO_SET: { status = cm_homing_cycle_start_no_set(); break;} // G28.4 - - case NEXT_ACTION_STRAIGHT_PROBE: { status = cm_straight_probe(cm.gn.target, cm.gf.target); break;} // G38.2 - - case NEXT_ACTION_SET_COORD_DATA: { status = cm_set_coord_offsets(cm.gn.parameter, cm.gn.target, cm.gf.target); break;} - case NEXT_ACTION_SET_ORIGIN_OFFSETS: { status = cm_set_origin_offsets(cm.gn.target, cm.gf.target); break;} - case NEXT_ACTION_RESET_ORIGIN_OFFSETS: { status = cm_reset_origin_offsets(); break;} - case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: { status = cm_suspend_origin_offsets(); break;} - case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: { status = cm_resume_origin_offsets(); break;} - - case NEXT_ACTION_DEFAULT: { - cm_set_absolute_override(MODEL, cm.gn.absolute_override); // apply override setting to gm struct - switch (cm.gn.motion_mode) { - case MOTION_MODE_CANCEL_MOTION_MODE: { cm.gm.motion_mode = cm.gn.motion_mode; break;} - case MOTION_MODE_STRAIGHT_TRAVERSE: { status = cm_straight_traverse(cm.gn.target, cm.gf.target); break;} - case MOTION_MODE_STRAIGHT_FEED: { status = cm_straight_feed(cm.gn.target, cm.gf.target); break;} - case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: - // gf.radius sets radius mode if radius was collected in gn - { status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], cm.gn.arc_offset[1], - cm.gn.arc_offset[2], cm.gn.arc_radius, cm.gn.motion_mode); break;} - } - } - } - cm_set_absolute_override(MODEL, false); // un-set absolute override once the move is planned - - // do the program stops and ends : M0, M1, M2, M30, M60 - if (cm.gf.program_flow == true) { - if (cm.gn.program_flow == PROGRAM_STOP) { - cm_program_stop(); - } else { - cm_program_end(); - } - } - return (status); + stat_t status = STAT_OK; + + cm_set_model_linenum(cm.gn.linenum); + EXEC_FUNC(cm_set_feed_rate_mode, feed_rate_mode); + EXEC_FUNC(cm_set_feed_rate, feed_rate); + EXEC_FUNC(cm_feed_rate_override_factor, feed_rate_override_factor); + EXEC_FUNC(cm_traverse_override_factor, traverse_override_factor); + EXEC_FUNC(cm_set_spindle_speed, spindle_speed); + EXEC_FUNC(cm_spindle_override_factor, spindle_override_factor); + EXEC_FUNC(cm_select_tool, tool_select); // tool_select is where it's written + EXEC_FUNC(cm_change_tool, tool_change); + EXEC_FUNC(cm_spindle_control, spindle_mode); // spindle on or off + EXEC_FUNC(cm_mist_coolant_control, mist_coolant); + EXEC_FUNC(cm_flood_coolant_control, flood_coolant); // also disables mist coolant if OFF + EXEC_FUNC(cm_feed_rate_override_enable, feed_rate_override_enable); + EXEC_FUNC(cm_traverse_override_enable, traverse_override_enable); + EXEC_FUNC(cm_spindle_override_enable, spindle_override_enable); + EXEC_FUNC(cm_override_enables, override_enables); + + if (cm.gn.next_action == NEXT_ACTION_DWELL) { // G4 - dwell + ritorno(cm_dwell(cm.gn.parameter)); // return if error, otherwise complete the block + } + EXEC_FUNC(cm_select_plane, select_plane); + EXEC_FUNC(cm_set_units_mode, units_mode); + //--> cutter radius compensation goes here + //--> cutter length compensation goes here + EXEC_FUNC(cm_set_coord_system, coord_system); + EXEC_FUNC(cm_set_path_control, path_control); + EXEC_FUNC(cm_set_distance_mode, distance_mode); + //--> set retract mode goes here + + switch (cm.gn.next_action) { + case NEXT_ACTION_SET_G28_POSITION: { status = cm_set_g28_position(); break;} // G28.1 + case NEXT_ACTION_GOTO_G28_POSITION: { status = cm_goto_g28_position(cm.gn.target, cm.gf.target); break;} // G28 + case NEXT_ACTION_SET_G30_POSITION: { status = cm_set_g30_position(); break;} // G30.1 + case NEXT_ACTION_GOTO_G30_POSITION: { status = cm_goto_g30_position(cm.gn.target, cm.gf.target); break;} // G30 + + case NEXT_ACTION_SEARCH_HOME: { status = cm_homing_cycle_start(); break;} // G28.2 + case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: { status = cm_set_absolute_origin(cm.gn.target, cm.gf.target); break;}// G28.3 + case NEXT_ACTION_HOMING_NO_SET: { status = cm_homing_cycle_start_no_set(); break;} // G28.4 + + case NEXT_ACTION_STRAIGHT_PROBE: { status = cm_straight_probe(cm.gn.target, cm.gf.target); break;} // G38.2 + + case NEXT_ACTION_SET_COORD_DATA: { status = cm_set_coord_offsets(cm.gn.parameter, cm.gn.target, cm.gf.target); break;} + case NEXT_ACTION_SET_ORIGIN_OFFSETS: { status = cm_set_origin_offsets(cm.gn.target, cm.gf.target); break;} + case NEXT_ACTION_RESET_ORIGIN_OFFSETS: { status = cm_reset_origin_offsets(); break;} + case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: { status = cm_suspend_origin_offsets(); break;} + case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: { status = cm_resume_origin_offsets(); break;} + + case NEXT_ACTION_DEFAULT: { + cm_set_absolute_override(MODEL, cm.gn.absolute_override); // apply override setting to gm struct + switch (cm.gn.motion_mode) { + case MOTION_MODE_CANCEL_MOTION_MODE: { cm.gm.motion_mode = cm.gn.motion_mode; break;} + case MOTION_MODE_STRAIGHT_TRAVERSE: { status = cm_straight_traverse(cm.gn.target, cm.gf.target); break;} + case MOTION_MODE_STRAIGHT_FEED: { status = cm_straight_feed(cm.gn.target, cm.gf.target); break;} + case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: + // gf.radius sets radius mode if radius was collected in gn + { status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], cm.gn.arc_offset[1], + cm.gn.arc_offset[2], cm.gn.arc_radius, cm.gn.motion_mode); break;} + } + } + } + cm_set_absolute_override(MODEL, false); // un-set absolute override once the move is planned + + // do the program stops and ends : M0, M1, M2, M30, M60 + if (cm.gf.program_flow == true) { + if (cm.gn.program_flow == PROGRAM_STOP) { + cm_program_stop(); + } else { + cm_program_end(); + } + } + return status; } @@ -517,12 +512,12 @@ static stat_t _execute_gcode_block() stat_t gc_get_gc(nvObj_t *nv) { - ritorno(nv_copy_string(nv, cs.saved_buf)); - nv->valuetype = TYPE_STRING; - return (STAT_OK); + ritorno(nv_copy_string(nv, cs.saved_buf)); + nv->valuetype = TYPE_STRING; + return STAT_OK; } stat_t gc_run_gc(nvObj_t *nv) { - return(gc_gcode_parser(*nv->stringp)); + return(gc_gcode_parser(*nv->stringp)); } diff --git a/src/gpio.c b/src/gpio.c index 6551e19..21876d2 100755 --- a/src/gpio.c +++ b/src/gpio.c @@ -25,25 +25,25 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* - * This GPIO file is where all parallel port bits are managed that are - * not already taken up by steppers, serial ports, SPI or PDI programming + * This GPIO file is where all parallel port bits are managed that are + * not already taken up by steppers, serial ports, SPI or PDI programming * - * There are 2 GPIO ports: + * There are 2 GPIO ports: * - * gpio1 Located on 5x2 header next to the PDI programming plugs (on v7's) - * Four (4) output bits capable of driving 3.3v or 5v logic + * gpio1 Located on 5x2 header next to the PDI programming plugs (on v7's) + * Four (4) output bits capable of driving 3.3v or 5v logic * - * Note: On v6 and earlier boards there are also 4 inputs: - * Four (4) level converted input bits capable of being driven - * by 3.3v or 5v logic - connected to B0 - B3 (now used for SPI) + * Note: On v6 and earlier boards there are also 4 inputs: + * Four (4) level converted input bits capable of being driven + * by 3.3v or 5v logic - connected to B0 - B3 (now used for SPI) * - * gpio2 Located on 9x2 header on "bottom" edge of the board - * Eight (8) non-level converted input bits - * Eight (8) ground pins - one each "under" each input pin - * Two (2) 3.3v power pins (on left edge of connector) - * Inputs can be used as switch contact inputs or - * 3.3v input bits depending on port configuration - * **** These bits CANNOT be used as 5v inputs **** + * gpio2 Located on 9x2 header on "bottom" edge of the board + * Eight (8) non-level converted input bits + * Eight (8) ground pins - one each "under" each input pin + * Two (2) 3.3v power pins (on left edge of connector) + * Inputs can be used as switch contact inputs or + * 3.3v input bits depending on port configuration + * **** These bits CANNOT be used as 5v inputs **** */ #include @@ -55,37 +55,37 @@ #include "hardware.h" #include "gpio.h" #include "canonical_machine.h" -#include "xio.h" // signals +#include "xio/xio.h" // signals //======================== Parallel IO Functions =============================== /* - * IndicatorLed_set() - fake out for IndicatorLed.set() until we get Motate running + * IndicatorLed_set() - fake out for IndicatorLed.set() until we get Motate running * IndicatorLed_clear() - fake out for IndicatorLed.clear() until we get Motate running * IndicatorLed_toggle()- fake out for IndicatorLed.toggle() until we get Motate running */ void IndicatorLed_set() { - gpio_led_on(INDICATOR_LED); - cs.led_state = 1; + gpio_led_on(INDICATOR_LED); + cs.led_state = 1; } void IndicatorLed_clear() { - gpio_led_off(INDICATOR_LED); - cs.led_state = 0; + gpio_led_off(INDICATOR_LED); + cs.led_state = 0; } void IndicatorLed_toggle() { - if (cs.led_state == 0) { - gpio_led_on(INDICATOR_LED); - cs.led_state = 1; - } else { - gpio_led_off(INDICATOR_LED); - cs.led_state = 0; - } + if (cs.led_state == 0) { + gpio_led_on(INDICATOR_LED); + cs.led_state = 1; + } else { + gpio_led_off(INDICATOR_LED); + cs.led_state = 0; + } } /* @@ -97,46 +97,46 @@ void IndicatorLed_toggle() void gpio_led_on(uint8_t led) { if (led == 0) gpio_set_bit_on(0x08); else - if (led == 1) gpio_set_bit_on(0x04); else - if (led == 2) gpio_set_bit_on(0x02); else - if (led == 3) gpio_set_bit_on(0x01); + if (led == 1) gpio_set_bit_on(0x04); else + if (led == 2) gpio_set_bit_on(0x02); else + if (led == 3) gpio_set_bit_on(0x01); } void gpio_led_off(uint8_t led) { if (led == 0) gpio_set_bit_off(0x08); else - if (led == 1) gpio_set_bit_off(0x04); else - if (led == 2) gpio_set_bit_off(0x02); else - if (led == 3) gpio_set_bit_off(0x01); + if (led == 1) gpio_set_bit_off(0x04); else + if (led == 2) gpio_set_bit_off(0x02); else + if (led == 3) gpio_set_bit_off(0x01); } void gpio_led_toggle(uint8_t led) { - if (led == 0) { - if (gpio_read_bit(0x08)) { - gpio_set_bit_off(0x08); - } else { - gpio_set_bit_on(0x08); - } - } else if (led == 1) { - if (gpio_read_bit(0x04)) { - gpio_set_bit_off(0x04); - } else { - gpio_set_bit_on(0x04); - } - } else if (led == 2) { - if (gpio_read_bit(0x02)) { - gpio_set_bit_off(0x02); - } else { - gpio_set_bit_on(0x02); - } - } else if (led == 3) { - if (gpio_read_bit(0x08)) { - gpio_set_bit_off(0x08); - } else { - gpio_set_bit_on(0x08); - } - } + if (led == 0) { + if (gpio_read_bit(0x08)) { + gpio_set_bit_off(0x08); + } else { + gpio_set_bit_on(0x08); + } + } else if (led == 1) { + if (gpio_read_bit(0x04)) { + gpio_set_bit_off(0x04); + } else { + gpio_set_bit_on(0x04); + } + } else if (led == 2) { + if (gpio_read_bit(0x02)) { + gpio_set_bit_off(0x02); + } else { + gpio_set_bit_on(0x02); + } + } else if (led == 3) { + if (gpio_read_bit(0x08)) { + gpio_set_bit_off(0x08); + } else { + gpio_set_bit_on(0x08); + } + } } /* @@ -144,30 +144,30 @@ void gpio_led_toggle(uint8_t led) * gpio_set_bit_on() - turn bit on * gpio_set_bit_off() - turn bit on * - * These functions have an inner remap depending on what hardware is running + * These functions have an inner remap depending on what hardware is running */ uint8_t gpio_read_bit(uint8_t b) { - if (b & 0x08) { return (hw.out_port[0]->IN & GPIO1_OUT_BIT_bm); } - if (b & 0x04) { return (hw.out_port[1]->IN & GPIO1_OUT_BIT_bm); } - if (b & 0x02) { return (hw.out_port[2]->IN & GPIO1_OUT_BIT_bm); } - if (b & 0x01) { return (hw.out_port[3]->IN & GPIO1_OUT_BIT_bm); } - return (0); + if (b & 0x08) { return hw.out_port[0]->IN & GPIO1_OUT_BIT_bm; } + if (b & 0x04) { return hw.out_port[1]->IN & GPIO1_OUT_BIT_bm; } + if (b & 0x02) { return hw.out_port[2]->IN & GPIO1_OUT_BIT_bm; } + if (b & 0x01) { return hw.out_port[3]->IN & GPIO1_OUT_BIT_bm; } + return 0; } void gpio_set_bit_on(uint8_t b) { - if (b & 0x08) { hw.out_port[0]->OUTSET = GPIO1_OUT_BIT_bm; } - if (b & 0x04) { hw.out_port[1]->OUTSET = GPIO1_OUT_BIT_bm; } - if (b & 0x02) { hw.out_port[2]->OUTSET = GPIO1_OUT_BIT_bm; } - if (b & 0x01) { hw.out_port[3]->OUTSET = GPIO1_OUT_BIT_bm; } + if (b & 0x08) { hw.out_port[0]->OUTSET = GPIO1_OUT_BIT_bm; } + if (b & 0x04) { hw.out_port[1]->OUTSET = GPIO1_OUT_BIT_bm; } + if (b & 0x02) { hw.out_port[2]->OUTSET = GPIO1_OUT_BIT_bm; } + if (b & 0x01) { hw.out_port[3]->OUTSET = GPIO1_OUT_BIT_bm; } } void gpio_set_bit_off(uint8_t b) { - if (b & 0x08) { hw.out_port[0]->OUTCLR = GPIO1_OUT_BIT_bm; } - if (b & 0x04) { hw.out_port[1]->OUTCLR = GPIO1_OUT_BIT_bm; } - if (b & 0x02) { hw.out_port[2]->OUTCLR = GPIO1_OUT_BIT_bm; } - if (b & 0x01) { hw.out_port[3]->OUTCLR = GPIO1_OUT_BIT_bm; } + if (b & 0x08) { hw.out_port[0]->OUTCLR = GPIO1_OUT_BIT_bm; } + if (b & 0x04) { hw.out_port[1]->OUTCLR = GPIO1_OUT_BIT_bm; } + if (b & 0x02) { hw.out_port[2]->OUTCLR = GPIO1_OUT_BIT_bm; } + if (b & 0x01) { hw.out_port[3]->OUTCLR = GPIO1_OUT_BIT_bm; } } diff --git a/src/gpio.h b/src/gpio.h index 73fec0e..ce0825f 100755 --- a/src/gpio.h +++ b/src/gpio.h @@ -28,9 +28,9 @@ #ifndef gpio_h #define gpio_h -void IndicatorLed_set(void); -void IndicatorLed_clear(void); -void IndicatorLed_toggle(void); +void IndicatorLed_set(); +void IndicatorLed_clear(); +void IndicatorLed_toggle(); void gpio_led_on(uint8_t led); void gpio_led_off(uint8_t led); diff --git a/src/hardware.c b/src/hardware.c index 4cbc537..df7ac08 100755 --- a/src/hardware.c +++ b/src/hardware.c @@ -25,10 +25,10 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include // used for software reset +#include // used for software reset -#include "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "hardware.h" #include "switch.h" #include "controller.h" @@ -44,102 +44,102 @@ static void _port_bindings(float hw_version) { hw.st_port[0] = &PORT_MOTOR_1; - hw.st_port[1] = &PORT_MOTOR_2; - hw.st_port[2] = &PORT_MOTOR_3; - hw.st_port[3] = &PORT_MOTOR_4; - - hw.sw_port[0] = &PORT_SWITCH_X; - hw.sw_port[1] = &PORT_SWITCH_Y; - hw.sw_port[2] = &PORT_SWITCH_Z; - hw.sw_port[3] = &PORT_SWITCH_A; - - if (hw_version > 6.9) { - hw.out_port[0] = &PORT_OUT_V7_X; - hw.out_port[1] = &PORT_OUT_V7_Y; - hw.out_port[2] = &PORT_OUT_V7_Z; - hw.out_port[3] = &PORT_OUT_V7_A; - } else { - hw.out_port[0] = &PORT_OUT_V6_X; - hw.out_port[1] = &PORT_OUT_V6_Y; - hw.out_port[2] = &PORT_OUT_V6_Z; - hw.out_port[3] = &PORT_OUT_V6_A; - } + hw.st_port[1] = &PORT_MOTOR_2; + hw.st_port[2] = &PORT_MOTOR_3; + hw.st_port[3] = &PORT_MOTOR_4; + + hw.sw_port[0] = &PORT_SWITCH_X; + hw.sw_port[1] = &PORT_SWITCH_Y; + hw.sw_port[2] = &PORT_SWITCH_Z; + hw.sw_port[3] = &PORT_SWITCH_A; + + if (hw_version > 6.9) { + hw.out_port[0] = &PORT_OUT_V7_X; + hw.out_port[1] = &PORT_OUT_V7_Y; + hw.out_port[2] = &PORT_OUT_V7_Z; + hw.out_port[3] = &PORT_OUT_V7_A; + } else { + hw.out_port[0] = &PORT_OUT_V6_X; + hw.out_port[1] = &PORT_OUT_V6_Y; + hw.out_port[2] = &PORT_OUT_V6_Z; + hw.out_port[3] = &PORT_OUT_V6_A; + } } void hardware_init() { - xmega_init(); // set system clock - _port_bindings(TINYG_HARDWARE_VERSION); - rtc_init(); // real time counter + xmega_init(); // set system clock + _port_bindings(TINYG_HARDWARE_VERSION); + rtc_init(); // real time counter } /* * _get_id() - get a human readable signature * - * Produce a unique deviceID based on the factory calibration data. - * Format is: 123456-ABC + * 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 - * The alpha is the low 5 bits of wafer number and XY coords in printable ASCII - * Refer to NVM_PROD_SIGNATURES_t in iox192a3.h for details. + * The number part is a direct readout of the 6 digit lot number + * The alpha is the low 5 bits of wafer number and XY coords in printable ASCII + * Refer to NVM_PROD_SIGNATURES_t in iox192a3.h for details. */ enum { - LOTNUM0=8, // Lot Number Byte 0, ASCII - LOTNUM1, // Lot Number Byte 1, ASCII - LOTNUM2, // Lot Number Byte 2, ASCII - LOTNUM3, // Lot Number Byte 3, ASCII - LOTNUM4, // Lot Number Byte 4, ASCII - LOTNUM5, // Lot Number Byte 5, ASCII - WAFNUM =16, // Wafer Number - COORDX0=18, // Wafer Coordinate X Byte 0 - COORDX1, // Wafer Coordinate X Byte 1 - COORDY0, // Wafer Coordinate Y Byte 0 - COORDY1, // Wafer Coordinate Y Byte 1 + LOTNUM0=8, // Lot Number Byte 0, ASCII + LOTNUM1, // Lot Number Byte 1, ASCII + LOTNUM2, // Lot Number Byte 2, ASCII + LOTNUM3, // Lot Number Byte 3, ASCII + LOTNUM4, // Lot Number Byte 4, ASCII + LOTNUM5, // Lot Number Byte 5, ASCII + WAFNUM =16, // Wafer Number + COORDX0=18, // Wafer Coordinate X Byte 0 + COORDX1, // Wafer Coordinate X Byte 1 + COORDY0, // Wafer Coordinate Y Byte 0 + COORDY1, // Wafer Coordinate Y Byte 1 }; static void _get_id(char_t *id) { char printable[33] = {"ABCDEFGHJKLMNPQRSTUVWXYZ23456789"}; - uint8_t i; - - NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; // Load NVM Command register to read the calibration row - - for (i=0; i<6; i++) { - id[i] = pgm_read_byte(LOTNUM0 + i); - } - id[i++] = '-'; - id[i++] = printable[(pgm_read_byte(WAFNUM) & 0x1F)]; - id[i++] = printable[(pgm_read_byte(COORDX0) & 0x1F)]; -// id[i++] = printable[(pgm_read_byte(COORDX1) & 0x1F)]; - id[i++] = printable[(pgm_read_byte(COORDY0) & 0x1F)]; -// id[i++] = printable[(pgm_read_byte(COORDY1) & 0x1F)]; - id[i] = 0; - - NVM_CMD = NVM_CMD_NO_OPERATION_gc; // Clean up NVM Command register + uint8_t i; + + NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; // Load NVM Command register to read the calibration row + + for (i=0; i<6; i++) { + id[i] = pgm_read_byte(LOTNUM0 + i); + } + id[i++] = '-'; + id[i++] = printable[(pgm_read_byte(WAFNUM) & 0x1F)]; + id[i++] = printable[(pgm_read_byte(COORDX0) & 0x1F)]; +// id[i++] = printable[(pgm_read_byte(COORDX1) & 0x1F)]; + id[i++] = printable[(pgm_read_byte(COORDY0) & 0x1F)]; +// id[i++] = printable[(pgm_read_byte(COORDY1) & 0x1F)]; + id[i] = 0; + + NVM_CMD = NVM_CMD_NO_OPERATION_gc; // Clean up NVM Command register } /* * Hardware Reset Handlers * * hw_request_hard_reset() - * hw_hard_reset() - hard reset using watchdog timer - * hw_hard_reset_handler() - controller's rest handler + * hw_hard_reset() - hard reset using watchdog timer + * hw_hard_reset_handler() - controller's rest handler */ void hw_request_hard_reset() { cs.hard_reset_requested = true; } -void hw_hard_reset(void) // software hard reset using the watchdog timer +void hw_hard_reset() // software hard reset using the watchdog timer { wdt_enable(WDTO_15MS); - while (true); // loops for about 15ms then resets + while (true); // loops for about 15ms then resets } -stat_t hw_hard_reset_handler(void) +stat_t hw_hard_reset_handler() { - if (cs.hard_reset_requested == false) - return (STAT_NOOP); - hw_hard_reset(); // hard reset - identical to hitting RESET button - return (STAT_EAGAIN); + if (cs.hard_reset_requested == false) + return STAT_NOOP; + hw_hard_reset(); // hard reset - identical to hitting RESET button + return STAT_EAGAIN; } /* @@ -151,14 +151,14 @@ stat_t hw_hard_reset_handler(void) void hw_request_bootloader() { cs.bootloader_requested = true;} -stat_t hw_bootloader_handler(void) +stat_t hw_bootloader_handler() { if (cs.bootloader_requested == false) - return (STAT_NOOP); - cli(); - CCPWrite(&RST.CTRL, RST_SWRST_bm); // fire a software reset + return STAT_NOOP; + cli(); + CCPWrite(&RST.CTRL, RST_SWRST_bm); // fire a software reset - return (STAT_EAGAIN); // never gets here but keeps the compiler happy + return STAT_EAGAIN; // never gets here but keeps the compiler happy } /***** END OF SYSTEM FUNCTIONS *****/ @@ -175,11 +175,11 @@ stat_t hw_bootloader_handler(void) stat_t hw_get_id(nvObj_t *nv) { - char_t tmp[SYS_ID_LEN]; - _get_id(tmp); - nv->valuetype = TYPE_STRING; - ritorno(nv_copy_string(nv, tmp)); - return (STAT_OK); + char_t tmp[SYS_ID_LEN]; + _get_id(tmp); + nv->valuetype = TYPE_STRING; + ritorno(nv_copy_string(nv, tmp)); + return STAT_OK; } /* @@ -187,8 +187,8 @@ stat_t hw_get_id(nvObj_t *nv) */ stat_t hw_run_boot(nvObj_t *nv) { - hw_request_bootloader(); - return(STAT_OK); + hw_request_bootloader(); + return(STAT_OK); } /* @@ -196,12 +196,12 @@ stat_t hw_run_boot(nvObj_t *nv) */ stat_t hw_set_hv(nvObj_t *nv) { - if (nv->value > TINYG_HARDWARE_VERSION_MAX) - return (STAT_INPUT_EXCEEDS_MAX_VALUE); - set_flt(nv); // record the hardware version - _port_bindings(nv->value); // reset port bindings - switch_init(); // re-initialize the GPIO ports - return (STAT_OK); + if (nv->value > TINYG_HARDWARE_VERSION_MAX) + return STAT_INPUT_EXCEEDS_MAX_VALUE; + set_flt(nv); // record the hardware version + _port_bindings(nv->value); // reset port bindings + switch_init(); // re-initialize the GPIO ports + return STAT_OK; } /*********************************************************************************** diff --git a/src/hardware.h b/src/hardware.h index 612cf1c..51514c1 100755 --- a/src/hardware.h +++ b/src/hardware.h @@ -1,6 +1,6 @@ /* * hardware.h - system hardware configuration - * THIS FILE IS HARDWARE PLATFORM SPECIFIC - AVR Xmega version + * THIS FILE IS HARDWARE PLATFORM SPECIFIC - AVR Xmega version * * This file is part of the TinyG project * @@ -30,19 +30,19 @@ /* * INTERRUPT USAGE - TinyG uses a lot of them all over the place * - * HI Stepper DDA pulse generation (set in stepper.h) - * HI Stepper load routine SW interrupt (set in stepper.h) - * HI Dwell timer counter (set in stepper.h) - * LO Segment execution SW interrupt (set in stepper.h) - * MED GPIO1 switch port (set in gpio.h) - * MED Serial RX for USB & RS-485 (set in xio_usart.h) - * MED Serial TX for USB & RS-485 (set in xio_usart.h) (* see note) - * LO Real time clock interrupt (set in xmega_rtc.h) + * HI Stepper DDA pulse generation (set in stepper.h) + * HI Stepper load routine SW interrupt (set in stepper.h) + * HI Dwell timer counter (set in stepper.h) + * LO Segment execution SW interrupt (set in stepper.h) + * MED GPIO1 switch port (set in gpio.h) + * MED Serial RX for USB & RS-485 (set in xio_usart.h) + * MED Serial TX for USB & RS-485 (set in xio_usart.h) (* see note) + * LO Real time clock interrupt (set in xmega_rtc.h) * - * (*) The TX cannot run at LO level or exception reports and other prints - * called from a LO interrupt (as in prep_line()) will kill the system in a - * permanent sleep_mode() call in xio_putc_usb() (xio.usb.c) as no interrupt - * can release the sleep mode. + * (*) The TX cannot run at LO level or exception reports and other prints + * called from a LO interrupt (as in prep_line()) will kill the system in a + * permanent sleep_mode() call in xio_putc_usb() (xio.usb.c) as no interrupt + * can release the sleep mode. */ #ifndef HARDWARE_H_ONCE @@ -51,85 +51,68 @@ /*--- Hardware platform enumerations ---*/ enum hwPlatform { - HM_PLATFORM_NONE = 0, + HM_PLATFORM_NONE = 0, - HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. - // hwVersion 7 = TinyG v7 and earlier - // hwVersion 8 = TinyG v8 + HW_PLATFORM_TINYG_XMEGA, // TinyG code base on Xmega boards. + // hwVersion 7 = TinyG v7 and earlier + // hwVersion 8 = TinyG v8 - HW_PLATFORM_G2_DUE, // G2 code base on native Arduino Due + HW_PLATFORM_G2_DUE, // G2 code base on native Arduino Due - HW_PLATFORM_TINYG_V9 // G2 code base on v9 boards - // hwVersion 0 = v9c - // hwVersion 1 = v9d - // hwVersion 2 = v9f - // hwVersion 3 = v9h - // hwVersion 4 = v9i + HW_PLATFORM_TINYG_V9 // G2 code base on v9 boards + // hwVersion 0 = v9c + // hwVersion 1 = v9d + // hwVersion 2 = v9f + // hwVersion 3 = v9h + // hwVersion 4 = v9i }; -#define HW_VERSION_TINYGV6 6 -#define HW_VERSION_TINYGV7 7 -#define HW_VERSION_TINYGV8 8 +#define HW_VERSION_TINYGV6 6 +#define HW_VERSION_TINYGV7 7 +#define HW_VERSION_TINYGV8 8 -#define HW_VERSION_TINYGV9C 0 -#define HW_VERSION_TINYGV9D 1 -#define HW_VERSION_TINYGV9F 2 -#define HW_VERSION_TINYGV9H 3 -#define HW_VERSION_TINYGV9I 4 +#define HW_VERSION_TINYGV9C 0 +#define HW_VERSION_TINYGV9D 1 +#define HW_VERSION_TINYGV9F 2 +#define HW_VERSION_TINYGV9H 3 +#define HW_VERSION_TINYGV9I 4 -//////////////////////////// -/////// AVR VERSION //////// -//////////////////////////// - -#include "config.h" // needed for the stat_t typedef +#include "config.h" // needed for the stat_t typedef #include -#include "xmega/xmega_rtc.h" // Xmega only. Goes away with RTC refactoring - -// uncomment once motate Xmega port is available -//#include "motatePins.h" -//#include "motateTimers.h" // for Motate::timer_number +#include "xmega/xmega_rtc.h" // Xmega only. Goes away with RTC refactoring -/************************* - * Global System Defines * - *************************/ +#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() -#undef F_CPU // CPU clock - set for delays -#define F_CPU 32000000UL // should always precede -#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() - -/************************************************************************************ - **** AVR XMEGA SPECIFIC HARDWARE *************************************************** - ************************************************************************************/ // 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 +//#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 /*** 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 -#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 -#define PORT_SWITCH_Y PORTD -#define PORT_SWITCH_Z PORTE -#define PORT_SWITCH_A PORTF - -#define PORT_OUT_V7_X PORTA // v7 mapping - Output bits mapped to ports -#define PORT_OUT_V7_Y PORTF -#define PORT_OUT_V7_Z PORTD -#define PORT_OUT_V7_A PORTE - -#define PORT_OUT_V6_X PORTA // v6 and earlier mapping - Output bits mapped to ports -#define PORT_OUT_V6_Y PORTF -#define PORT_OUT_V6_Z PORTE -#define PORT_OUT_V6_A PORTD +#define PORT_MOTOR_1 PORTA // motors mapped to ports +#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 +#define PORT_SWITCH_Y PORTD +#define PORT_SWITCH_Z PORTE +#define PORT_SWITCH_A PORTF + +#define PORT_OUT_V7_X PORTA // v7 mapping - Output bits mapped to ports +#define PORT_OUT_V7_Y PORTF +#define PORT_OUT_V7_Z PORTD +#define PORT_OUT_V7_A PORTE + +#define PORT_OUT_V6_X PORTA // v6 and earlier mapping - Output bits mapped to ports +#define PORT_OUT_V6_Y PORTF +#define PORT_OUT_V6_Z PORTE +#define PORT_OUT_V6_A PORTD // These next four must be changed when the PORT_MOTOR_* definitions change! #define PORTCFG_VP0MAP_PORT_MOTOR_1_gc PORTCFG_VP0MAP_PORTA_gc @@ -137,157 +120,150 @@ enum hwPlatform { #define PORTCFG_VP2MAP_PORT_MOTOR_3_gc PORTCFG_VP2MAP_PORTE_gc #define PORTCFG_VP3MAP_PORT_MOTOR_4_gc PORTCFG_VP3MAP_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 +#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) - * b3 (out) microstep 0 - * b4 (out) microstep 1 - * b5 (out) output bit for GPIO port1 - * b6 (in) min limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) - * b7 (in) max limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) + * b0 (out) step (SET is step, CLR is rest) + * b1 (out) direction (CLR = Clockwise) + * b2 (out) motor enable (CLR = Enabled) + * b3 (out) microstep 0 + * b4 (out) microstep 1 + * b5 (out) output bit for GPIO port1 + * b6 (in) min limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) + * b7 (in) max limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) */ -#define MOTOR_PORT_DIR_gm 0x3F // dir settings: lower 6 out, upper 2 in -//#define MOTOR_PORT_DIR_gm 0x00 // dir settings: all inputs - -enum cfgPortBits { // motor control port bit positions - STEP_BIT_bp = 0, // bit 0 - DIRECTION_BIT_bp, // bit 1 - MOTOR_ENABLE_BIT_bp, // bit 2 - MICROSTEP_BIT_0_bp, // bit 3 - MICROSTEP_BIT_1_bp, // bit 4 - GPIO1_OUT_BIT_bp, // bit 5 (4 gpio1 output bits; 1 from each axis) - SW_MIN_BIT_bp, // bit 6 (4 input bits for homing/limit switches) - SW_MAX_BIT_bp // bit 7 (4 input bits for homing/limit switches) +#define MOTOR_PORT_DIR_gm 0x3F // dir settings: lower 6 out, upper 2 in + +enum cfgPortBits { // motor control port bit positions + STEP_BIT_bp = 0, // bit 0 + DIRECTION_BIT_bp, // bit 1 + MOTOR_ENABLE_BIT_bp, // bit 2 + MICROSTEP_BIT_0_bp, // bit 3 + MICROSTEP_BIT_1_bp, // bit 4 + GPIO1_OUT_BIT_bp, // bit 5 (4 gpio1 output bits; 1 from each axis) + SW_MIN_BIT_bp, // bit 6 (4 input bits for homing/limit switches) + SW_MAX_BIT_bp // bit 7 (4 input bits for homing/limit switches) }; -#define STEP_BIT_bm (1< STAT_EAGAIN) { // erred out - return (status); - } - // propagate the group from previous NV pair (if relevant) - if (group[0] != NUL) { - strncpy(nv->group, group, GROUP_LEN); // copy the parent's group to this child - } - // validate the token and get the index - if ((nv->index = nv_get_index(nv->group, nv->token)) == NO_MATCH) { - return (STAT_UNRECOGNIZED_NAME); - } - if ((nv_index_is_group(nv->index)) && (nv_group_is_prefixed(nv->token))) { - strncpy(group, nv->token, GROUP_LEN); // record the group ID - } - if ((nv = nv->nx) == NULL) - return (STAT_JSON_TOO_MANY_PAIRS); // Not supposed to encounter a NULL - } while (status != STAT_OK); // breaks when parsing is complete - - // execute the command - nv = nv_body; - if (nv->valuetype == TYPE_NULL){ // means GET the value - ritorno(nv_get(nv)); // ritorno returns w/status on any errors - } else { - if (cm.machine_state == MACHINE_ALARM) - return (STAT_MACHINE_ALARMED); - ritorno(nv_set(nv)); // set value or call a function (e.g. gcode) - nv_persist(nv); - } - return (STAT_OK); // only successful commands exit through this point + if ((status = _get_nv_pair(nv, &str, &depth)) > STAT_EAGAIN) { // erred out + return status; + } + // propagate the group from previous NV pair (if relevant) + if (group[0] != 0) { + strncpy(nv->group, group, GROUP_LEN); // copy the parent's group to this child + } + // validate the token and get the index + if ((nv->index = nv_get_index(nv->group, nv->token)) == NO_MATCH) { + return STAT_UNRECOGNIZED_NAME; + } + if ((nv_index_is_group(nv->index)) && (nv_group_is_prefixed(nv->token))) { + strncpy(group, nv->token, GROUP_LEN); // record the group ID + } + if ((nv = nv->nx) == 0) + return STAT_JSON_TOO_MANY_PAIRS; // Not supposed to encounter a 0 + } while (status != STAT_OK); // breaks when parsing is complete + + // execute the command + nv = nv_body; + if (nv->valuetype == TYPE_0){ // means GET the value + ritorno(nv_get(nv)); // ritorno returns w/status on any errors + } else { + if (cm.machine_state == MACHINE_ALARM) + return STAT_MACHINE_ALARMED; + ritorno(nv_set(nv)); // set value or call a function (e.g. gcode) + nv_persist(nv); + } + return STAT_OK; // only successful commands exit through this point } /* * _normalize_json_string - normalize a JSON string in place * - * Validate string size limits, remove all whitespace and convert - * to lower case, with the exception of gcode comments + * Validate string size limits, remove all whitespace and convert + * to lower case, with the exception of gcode comments */ static stat_t _normalize_json_string(char_t *str, uint16_t size) { - char_t *wr; // write pointer - uint8_t in_comment = false; - - if (strlen(str) > size) - return (STAT_INPUT_EXCEEDS_MAX_LENGTH); - - for (wr = str; *str != NUL; str++) { - if (!in_comment) { // normal processing - if (*str == '(') in_comment = true; - if ((*str <= ' ') || (*str == DEL)) continue; // toss ctrls, WS & DEL - *wr++ = tolower(*str); - } else { // Gcode comment processing - if (*str == ')') in_comment = false; - *wr++ = *str; - } - } - *wr = NUL; - return (STAT_OK); + char_t *wr; // write pointer + uint8_t in_comment = false; + + if (strlen(str) > size) + return STAT_INPUT_EXCEEDS_MAX_LENGTH; + + for (wr = str; *str != 0; str++) { + if (!in_comment) { // normal processing + if (*str == '(') in_comment = true; + if ((*str <= ' ') || (*str == DEL)) continue; // toss ctrls, WS & DEL + *wr++ = tolower(*str); + } else { // Gcode comment processing + if (*str == ')') in_comment = false; + *wr++ = *str; + } + } + *wr = 0; + return STAT_OK; } /* * _get_nv_pair() - get the next name-value pair w/relaxed JSON rules. Also parses strict JSON. * - * Parse the next statement and populate the command object (nvObj). + * Parse the next statement and populate the command object (nvObj). * - * Leaves string pointer (str) on the first character following the object. - * Which is the character just past the ',' separator if it's a multi-valued - * object or the terminating NUL if single object or the last in a multi. + * Leaves string pointer (str) on the first character following the object. + * Which is the character just past the ',' separator if it's a multi-valued + * object or the terminating 0 if single object or the last in a multi. * - * Keeps track of tree depth and closing braces as much as it has to. - * If this were to be extended to track multiple parents or more than two - * levels deep it would have to track closing curlies - which it does not. + * Keeps track of tree depth and closing braces as much as it has to. + * If this were to be extended to track multiple parents or more than two + * levels deep it would have to track closing curlies - which it does not. * - * ASSUMES INPUT STRING HAS FIRST BEEN NORMALIZED BY _normalize_json_string() + * ASSUMES INPUT STRING HAS FIRST BEEN NORMALIZED BY _normalize_json_string() * - * If a group prefix is passed in it will be pre-pended to any name parsed - * to form a token string. For example, if "x" is provided as a group and - * "fr" is found in the name string the parser will search for "xfr" in the - * cfgArray. + * If a group prefix is passed in it will be pre-pended to any name parsed + * to form a token string. For example, if "x" is provided as a group and + * "fr" is found in the name string the parser will search for "xfr" in the + * cfgArray. */ -/* RELAXED RULES +/* RELAXED RULES * - * Quotes are accepted but not needed on names - * Quotes are required for string values + * Quotes are accepted but not needed on names + * Quotes are required for string values * - * See build 406.xx or earlier for strict JSON parser - deleted in 407.03 + * See build 406.xx or earlier for strict JSON parser - deleted in 407.03 */ #define MAX_PAD_CHARS 8 @@ -200,240 +200,240 @@ static stat_t _normalize_json_string(char_t *str, uint16_t size) static stat_t _get_nv_pair(nvObj_t *nv, char_t **pstr, int8_t *depth) { - uint8_t i; - char_t *tmp; - char_t leaders[] = {"{,\""}; // open curly, quote and leading comma - char_t separators[] = {":\""}; // colon and quote - char_t terminators[] = {"},\""}; // close curly, comma and quote - char_t value[] = {"{\".-+"}; // open curly, quote, period, minus and plus - - nv_reset_nv(nv); // wipes the object and sets the depth - - // --- Process name part --- - // Find, terminate and set pointers for the name. Allow for leading and trailing name quotes. - char_t * name = *pstr; - for (i=0; true; i++, (*pstr)++) { - if (strchr(leaders, (int)**pstr) == NULL) { // find leading character of name - name = (*pstr)++; - break; - } - if (i == MAX_PAD_CHARS) - return (STAT_JSON_SYNTAX_ERROR); - } - - // Find the end of name, NUL terminate and copy token - for (i=0; true; i++, (*pstr)++) { - if (strchr(separators, (int)**pstr) != NULL) { - *(*pstr)++ = NUL; - strncpy(nv->token, name, TOKEN_LEN+1); // copy the string to the token - break; - } - if (i == MAX_NAME_CHARS) - return (STAT_JSON_SYNTAX_ERROR); - } - - // --- Process value part --- (organized from most to least frequently encountered) - - // Find the start of the value part - for (i=0; true; i++, (*pstr)++) { - if (isalnum((int)**pstr)) break; - if (strchr(value, (int)**pstr) != NULL) break; - if (i == MAX_PAD_CHARS) - return (STAT_JSON_SYNTAX_ERROR); - } - - // nulls (gets) - if ((**pstr == 'n') || ((**pstr == '\"') && (*(*pstr+1) == '\"'))) { // process null value - nv->valuetype = TYPE_NULL; - nv->value = TYPE_NULL; - - // numbers - } else if (isdigit(**pstr) || (**pstr == '-')) {// value is a number - nv->value = (float)strtod(*pstr, &tmp); // tmp is the end pointer - if(tmp == *pstr) - return (STAT_BAD_NUMBER_FORMAT); - nv->valuetype = TYPE_FLOAT; - - // object parent - } else if (**pstr == '{') { - nv->valuetype = TYPE_PARENT; -// *depth += 1; // nv_reset_nv() sets the next object's level so this is redundant - (*pstr)++; - return(STAT_EAGAIN); // signal that there is more to parse - - // strings - } else if (**pstr == '\"') { // value is a string - (*pstr)++; - nv->valuetype = TYPE_STRING; - if ((tmp = strchr(*pstr, '\"')) == NULL) - return (STAT_JSON_SYNTAX_ERROR); // find the end of the string - *tmp = NUL; - - // if string begins with 0x it might be data, needs to be at least 3 chars long - if( strlen(*pstr)>=3 && (*pstr)[0]=='0' && (*pstr)[1]=='x') - { - uint32_t *v = (uint32_t*)&nv->value; - *v = strtoul((const char *)*pstr, 0L, 0); - nv->valuetype = TYPE_DATA; - } else { - ritorno(nv_copy_string(nv, *pstr)); - } - - *pstr = ++tmp; - - // boolean true/false - } else if (**pstr == 't') { - nv->valuetype = TYPE_BOOL; - nv->value = true; - } else if (**pstr == 'f') { - nv->valuetype = TYPE_BOOL; - nv->value = false; - - // arrays - } else if (**pstr == '[') { - nv->valuetype = TYPE_ARRAY; - ritorno(nv_copy_string(nv, *pstr)); // copy array into string for error displays - return (STAT_UNSUPPORTED_TYPE); // return error as the parser doesn't do input arrays yet - - // general error condition - } else { - return (STAT_JSON_SYNTAX_ERROR); // ill-formed JSON + uint8_t i; + char_t *tmp; + char_t leaders[] = {"{,\""}; // open curly, quote and leading comma + char_t separators[] = {":\""}; // colon and quote + char_t terminators[] = {"},\""}; // close curly, comma and quote + char_t value[] = {"{\".-+"}; // open curly, quote, period, minus and plus + + nv_reset_nv(nv); // wipes the object and sets the depth + + // --- Process name part --- + // Find, terminate and set pointers for the name. Allow for leading and trailing name quotes. + char_t * name = *pstr; + for (i=0; true; i++, (*pstr)++) { + if (strchr(leaders, (int)**pstr) == 0) { // find leading character of name + name = (*pstr)++; + break; + } + if (i == MAX_PAD_CHARS) + return STAT_JSON_SYNTAX_ERROR; + } + + // Find the end of name, 0 terminate and copy token + for (i=0; true; i++, (*pstr)++) { + if (strchr(separators, (int)**pstr) != 0) { + *(*pstr)++ = 0; + strncpy(nv->token, name, TOKEN_LEN+1); // copy the string to the token + break; + } + if (i == MAX_NAME_CHARS) + return STAT_JSON_SYNTAX_ERROR; + } + + // --- Process value part --- (organized from most to least frequently encountered) + + // Find the start of the value part + for (i=0; true; i++, (*pstr)++) { + if (isalnum((int)**pstr)) break; + if (strchr(value, (int)**pstr) != 0) break; + if (i == MAX_PAD_CHARS) + return STAT_JSON_SYNTAX_ERROR; + } + + // nulls (gets) + if ((**pstr == 'n') || ((**pstr == '\"') && (*(*pstr+1) == '\"'))) { // process null value + nv->valuetype = TYPE_0; + nv->value = TYPE_0; + + // numbers + } else if (isdigit(**pstr) || (**pstr == '-')) {// value is a number + nv->value = (float)strtod(*pstr, &tmp); // tmp is the end pointer + if(tmp == *pstr) + return STAT_BAD_NUMBER_FORMAT; + nv->valuetype = TYPE_FLOAT; + + // object parent + } else if (**pstr == '{') { + nv->valuetype = TYPE_PARENT; +// *depth += 1; // nv_reset_nv() sets the next object's level so this is redundant + (*pstr)++; + return(STAT_EAGAIN); // signal that there is more to parse + + // strings + } else if (**pstr == '\"') { // value is a string + (*pstr)++; + nv->valuetype = TYPE_STRING; + if ((tmp = strchr(*pstr, '\"')) == 0) + return STAT_JSON_SYNTAX_ERROR; // find the end of the string + *tmp = 0; + + // if string begins with 0x it might be data, needs to be at least 3 chars long + if( strlen(*pstr)>=3 && (*pstr)[0]=='0' && (*pstr)[1]=='x') + { + uint32_t *v = (uint32_t*)&nv->value; + *v = strtoul((const char *)*pstr, 0L, 0); + nv->valuetype = TYPE_DATA; + } else { + ritorno(nv_copy_string(nv, *pstr)); + } + + *pstr = ++tmp; + + // boolean true/false + } else if (**pstr == 't') { + nv->valuetype = TYPE_BOOL; + nv->value = true; + } else if (**pstr == 'f') { + nv->valuetype = TYPE_BOOL; + nv->value = false; + + // arrays + } else if (**pstr == '[') { + nv->valuetype = TYPE_ARRAY; + ritorno(nv_copy_string(nv, *pstr)); // copy array into string for error displays + return STAT_UNSUPPORTED_TYPE; // return error as the parser doesn't do input arrays yet + + // general error condition + } else { + return STAT_JSON_SYNTAX_ERROR; // ill-formed JSON } - // process comma separators and end curlies - if ((*pstr = strpbrk(*pstr, terminators)) == NULL) { // advance to terminator or err out - return (STAT_JSON_SYNTAX_ERROR); - } - if (**pstr == '}') { - *depth -= 1; // pop up a nesting level - (*pstr)++; // advance to comma or whatever follows - } - if (**pstr == ',') - return (STAT_EAGAIN); // signal that there is more to parse - - (*pstr)++; - return (STAT_OK); // signal that parsing is complete + // process comma separators and end curlies + if ((*pstr = strpbrk(*pstr, terminators)) == 0) { // advance to terminator or err out + return STAT_JSON_SYNTAX_ERROR; + } + if (**pstr == '}') { + *depth -= 1; // pop up a nesting level + (*pstr)++; // advance to comma or whatever follows + } + if (**pstr == ',') + return STAT_EAGAIN; // signal that there is more to parse + + (*pstr)++; + return STAT_OK; // signal that parsing is complete } /**************************************************************************** * json_serialize() - make a JSON object string from JSON object array * - * *nv is a pointer to the first element in the nv list to serialize - * *out_buf is a pointer to the output string - usually what was the input string - * Returns the character count of the resulting string + * *nv is a pointer to the first element in the nv list to serialize + * *out_buf is a pointer to the output string - usually what was the input string + * Returns the character count of the resulting string * - * Operation: - * - The nvObj list is processed start to finish with no recursion + * Operation: + * - The nvObj list is processed start to finish with no recursion * - * - Assume the first object is depth 0 or greater (the opening curly) + * - Assume the first object is depth 0 or greater (the opening curly) * - * - Assume remaining depths have been set correctly; but might not achieve closure; - * e.g. list starts on 0, and ends on 3, in which case provide correct closing curlies + * - Assume remaining depths have been set correctly; but might not achieve closure; + * e.g. list starts on 0, and ends on 3, in which case provide correct closing curlies * - * - Assume there can be multiple, independent, non-contiguous JSON objects at a - * given depth value. These are processed correctly - e.g. 0,1,1,0,1,1,0,1,1 + * - Assume there can be multiple, independent, non-contiguous JSON objects at a + * given depth value. These are processed correctly - e.g. 0,1,1,0,1,1,0,1,1 * - * - The list must have a terminating nvObj where nv->nx == NULL. - * The terminating object may or may not have data (empty or not empty). + * - The list must have a terminating nvObj where nv->nx == 0. + * The terminating object may or may not have data (empty or not empty). * - * Returns: - * Returns length of string + * Returns: + * Returns length of string * - * Desired behaviors: - * - Allow self-referential elements that would otherwise cause a recursive loop - * - Skip over empty objects (TYPE_EMPTY) - * - If a JSON object is empty represent it as {} - * --- OR --- - * - If a JSON object is empty omit the object altogether (no curlies) + * Desired behaviors: + * - Allow self-referential elements that would otherwise cause a recursive loop + * - Skip over empty objects (TYPE_EMPTY) + * - If a JSON object is empty represent it as {} + * --- OR --- + * - If a JSON object is empty omit the object altogether (no curlies) */ -#define BUFFER_MARGIN 8 // safety margin to avoid buffer overruns during footer checksum generation +#define BUFFER_MARGIN 8 // safety margin to avoid buffer overruns during footer checksum generation uint16_t json_serialize(nvObj_t *nv, char_t *out_buf, uint16_t size) { #ifdef __SILENCE_JSON_RESPONSES - return (0); + return 0; #else - char_t *str = out_buf; - char_t *str_max = out_buf + size - BUFFER_MARGIN; - int8_t initial_depth = nv->depth; - int8_t prev_depth = 0; - uint8_t need_a_comma = false; - - *str++ = '{'; // write opening curly - - while (true) { - if (nv->valuetype != TYPE_EMPTY) { - if (need_a_comma) { *str++ = ',';} - need_a_comma = true; - if (js.json_syntax == JSON_SYNTAX_RELAXED) { // write name - str += sprintf((char *)str, "%s:", nv->token); - } else { - str += sprintf((char *)str, "\"%s\":", nv->token); - } - - // check for illegal float values - if (nv->valuetype == TYPE_FLOAT) { - if (isnan((double)nv->value) || isinf((double)nv->value)) { nv->value = 0;} - } - - // serialize output value - if (nv->valuetype == TYPE_NULL) { str += (char_t)sprintf((char *)str, "null");} // Note that that "" is NOT null. - else if (nv->valuetype == TYPE_INTEGER) { - str += (char_t)sprintf((char *)str, "%1.0f", (double)nv->value); - } - else if (nv->valuetype == TYPE_DATA) { - uint32_t *v = (uint32_t*)&nv->value; - str += (char_t)sprintf((char *)str, "\"0x%lx\"", *v); - } - else if (nv->valuetype == TYPE_STRING) { str += (char_t)sprintf((char *)str, "\"%s\"",(char *)*nv->stringp);} - else if (nv->valuetype == TYPE_ARRAY) { str += (char_t)sprintf((char *)str, "[%s]", (char *)*nv->stringp);} - else if (nv->valuetype == TYPE_FLOAT) { preprocess_float(nv); -// str += fntoa((char *)str, nv->value, nv->precision); - str += fntoa(str, nv->value, nv->precision); - } - else if (nv->valuetype == TYPE_BOOL) { - if (fp_FALSE(nv->value)) { str += sprintf((char *)str, "false");} - else { str += (char_t)sprintf((char *)str, "true"); } - } - if (nv->valuetype == TYPE_PARENT) { - *str++ = '{'; - need_a_comma = false; - } - } - if (str >= str_max) { return (-1);} // signal buffer overrun - if ((nv = nv->nx) == NULL) { break;} // end of the list - - while (nv->depth < prev_depth--) { // iterate the closing curlies - need_a_comma = true; - *str++ = '}'; - } - prev_depth = nv->depth; - } - - // closing curlies and NEWLINE - while (prev_depth-- > initial_depth) { *str++ = '}';} - str += sprintf((char *)str, "}\n"); // using sprintf for this last one ensures a NUL termination - if (str > out_buf + size) { return (-1);} - return (str - out_buf); + char_t *str = out_buf; + char_t *str_max = out_buf + size - BUFFER_MARGIN; + int8_t initial_depth = nv->depth; + int8_t prev_depth = 0; + uint8_t need_a_comma = false; + + *str++ = '{'; // write opening curly + + while (true) { + if (nv->valuetype != TYPE_EMPTY) { + if (need_a_comma) { *str++ = ',';} + need_a_comma = true; + if (js.json_syntax == JSON_SYNTAX_RELAXED) { // write name + str += sprintf((char *)str, "%s:", nv->token); + } else { + str += sprintf((char *)str, "\"%s\":", nv->token); + } + + // check for illegal float values + if (nv->valuetype == TYPE_FLOAT) { + if (isnan((double)nv->value) || isinf((double)nv->value)) { nv->value = 0;} + } + + // serialize output value + if (nv->valuetype == TYPE_0) { str += (char_t)sprintf((char *)str, "null");} // Note that that "" is NOT null. + else if (nv->valuetype == TYPE_INTEGER) { + str += (char_t)sprintf((char *)str, "%1.0f", (double)nv->value); + } + else if (nv->valuetype == TYPE_DATA) { + uint32_t *v = (uint32_t*)&nv->value; + str += (char_t)sprintf((char *)str, "\"0x%lx\"", *v); + } + else if (nv->valuetype == TYPE_STRING) { str += (char_t)sprintf((char *)str, "\"%s\"",(char *)*nv->stringp);} + else if (nv->valuetype == TYPE_ARRAY) { str += (char_t)sprintf((char *)str, "[%s]", (char *)*nv->stringp);} + else if (nv->valuetype == TYPE_FLOAT) { preprocess_float(nv); +// str += fntoa((char *)str, nv->value, nv->precision); + str += fntoa(str, nv->value, nv->precision); + } + else if (nv->valuetype == TYPE_BOOL) { + if (fp_FALSE(nv->value)) { str += sprintf((char *)str, "false");} + else { str += (char_t)sprintf((char *)str, "true"); } + } + if (nv->valuetype == TYPE_PARENT) { + *str++ = '{'; + need_a_comma = false; + } + } + if (str >= str_max) { return -1;} // signal buffer overrun + if ((nv = nv->nx) == 0) { break;} // end of the list + + while (nv->depth < prev_depth--) { // iterate the closing curlies + need_a_comma = true; + *str++ = '}'; + } + prev_depth = nv->depth; + } + + // closing curlies and NEWLINE + while (prev_depth-- > initial_depth) { *str++ = '}';} + str += sprintf((char *)str, "}\n"); // using sprintf for this last one ensures a 0 termination + if (str > out_buf + size) { return -1;} + return str - out_buf; #endif } /* * json_print_object() - serialize and print the nvObj array directly (w/o header & footer) * - * Ignores JSON verbosity settings and everything else - just serializes the list & prints - * Useful for reports and other simple output. - * Object list should be terminated by nv->nx == NULL + * Ignores JSON verbosity settings and everything else - just serializes the list & prints + * Useful for reports and other simple output. + * Object list should be terminated by nv->nx == 0 */ void json_print_object(nvObj_t *nv) { #ifdef __SILENCE_JSON_RESPONSES - return; + return; #endif - json_serialize(nv, cs.out_buf, sizeof(cs.out_buf)); - fprintf(stderr, "%s", (char *)cs.out_buf); + json_serialize(nv, cs.out_buf, sizeof(cs.out_buf)); + fprintf(stderr, "%s", (char *)cs.out_buf); } /* @@ -442,106 +442,106 @@ void json_print_object(nvObj_t *nv) void json_print_list(stat_t status, uint8_t flags) { - switch (flags) { - case JSON_NO_PRINT: { break; } - case JSON_OBJECT_FORMAT: { json_print_object(nv_body); break; } - case JSON_RESPONSE_FORMAT: { json_print_response(status); break; } - } + switch (flags) { + case JSON_NO_PRINT: { break; } + case JSON_OBJECT_FORMAT: { json_print_object(nv_body); break; } + case JSON_RESPONSE_FORMAT: { json_print_response(status); break; } + } } /* * json_print_response() - JSON responses with headers, footers and observing JSON verbosity * - * A footer is returned for every setting except $jv=0 + * A footer is returned for every setting except $jv=0 * - * JV_SILENT = 0, // no response is provided for any command - * JV_FOOTER, // responses contain footer only; no command echo, gcode blocks or messages - * JV_CONFIGS, // echo configs; gcode blocks are not echoed; messages are not echoed - * JV_MESSAGES, // echo configs; gcode messages only (if present); no block echo or line numbers - * JV_LINENUM, // echo configs; gcode blocks return messages and line numbers as present - * JV_VERBOSE // echos all configs and gcode blocks, line numbers and messages + * JV_SILENT = 0, // no response is provided for any command + * JV_FOOTER, // responses contain footer only; no command echo, gcode blocks or messages + * JV_CONFIGS, // echo configs; gcode blocks are not echoed; messages are not echoed + * JV_MESSAGES, // echo configs; gcode messages only (if present); no block echo or line numbers + * JV_LINENUM, // echo configs; gcode blocks return messages and line numbers as present + * JV_VERBOSE // echos all configs and gcode blocks, line numbers and messages * - * This gets a bit complicated. The first nvObj is the header, which must be set by reset_nv_list(). - * The first object in the body will always have the gcode block or config command in it, - * which you may or may not want to display. This is followed by zero or more displayable objects. - * Then if you want a gcode line number you add that here to the end. Finally, a footer goes - * on all the (non-silent) responses. + * This gets a bit complicated. The first nvObj is the header, which must be set by reset_nv_list(). + * The first object in the body will always have the gcode block or config command in it, + * which you may or may not want to display. This is followed by zero or more displayable objects. + * Then if you want a gcode line number you add that here to the end. Finally, a footer goes + * on all the (non-silent) responses. */ #define MAX_TAIL_LEN 8 void json_print_response(uint8_t status) { #ifdef __SILENCE_JSON_RESPONSES - return; + return; #endif - if (js.json_verbosity == JV_SILENT) return; // silent responses - - // Body processing - nvObj_t *nv = nv_body; - if (status == STAT_JSON_SYNTAX_ERROR) { - nv_reset_nv_list(); - nv_add_string((const char_t *)"err", escape_string(cs.in_buf, cs.saved_buf)); - - } else if (cm.machine_state != MACHINE_INITIALIZING) { // always do full echo during startup - uint8_t nv_type; - do { - if ((nv_type = nv_get_type(nv)) == NV_TYPE_NULL) break; - - if (nv_type == NV_TYPE_GCODE) { - if (js.echo_json_gcode_block == false) { // kill command echo if not enabled - nv->valuetype = TYPE_EMPTY; - } - -//++++ } else if (nv_type == NV_TYPE_CONFIG) { // kill config echo if not enabled -//fix me if (js.echo_json_configs == false) { -// nv->valuetype = TYPE_EMPTY; -// } - - } else if (nv_type == NV_TYPE_MESSAGE) { // kill message echo if not enabled - if (js.echo_json_messages == false) { - nv->valuetype = TYPE_EMPTY; - } - - } else if (nv_type == NV_TYPE_LINENUM) { // kill line number echo if not enabled - if ((js.echo_json_linenum == false) || (fp_ZERO(nv->value))) { // do not report line# 0 - nv->valuetype = TYPE_EMPTY; - } - } - } while ((nv = nv->nx) != NULL); - } - - // Footer processing - while(nv->valuetype != TYPE_EMPTY) { // find a free nvObj at end of the list... - if ((nv = nv->nx) == NULL) { //...or hit the NULL and return w/o a footer - json_serialize(nv_header, cs.out_buf, sizeof(cs.out_buf)); - return; - } - } - char_t footer_string[NV_FOOTER_LEN]; - sprintf((char *)footer_string, "%d,%d,%d,0", FOOTER_REVISION, status, cs.linelen); - cs.linelen = 0; // reset linelen so it's only reported once - - nv_copy_string(nv, footer_string); // link string to nv object -// nv->depth = 0; // footer 'f' is a peer to response 'r' (hard wired to 0) - nv->depth = js.json_footer_depth; // 0=footer is peer to response 'r', 1=child of response 'r' - nv->valuetype = TYPE_ARRAY; - strcpy(nv->token, "f"); // terminate the list - nv->nx = NULL; - - // do all this to avoid having to serialize it twice - int16_t strcount = json_serialize(nv_header, cs.out_buf, sizeof(cs.out_buf));// make JSON string w/o checksum - if (strcount < 0) { return;} // encountered an overrun during serialization - if (strcount > OUTPUT_BUFFER_LEN - MAX_TAIL_LEN) { return;} // would overrun during checksum generation - int16_t strcount2 = strcount; - char tail[MAX_TAIL_LEN]; - - while (cs.out_buf[strcount] != '0') { strcount--; } // find end of checksum - strcpy(tail, cs.out_buf + strcount + 1); // save the json termination - - while (cs.out_buf[strcount2] != ',') { strcount2--; }// find start of checksum - sprintf((char *)cs.out_buf + strcount2 + 1, "%d%s", compute_checksum(cs.out_buf, strcount2), tail); - fprintf(stderr, "%s", cs.out_buf); + if (js.json_verbosity == JV_SILENT) return; // silent responses + + // Body processing + nvObj_t *nv = nv_body; + if (status == STAT_JSON_SYNTAX_ERROR) { + nv_reset_nv_list(); + nv_add_string((const char_t *)"err", escape_string(cs.in_buf, cs.saved_buf)); + + } else if (cm.machine_state != MACHINE_INITIALIZING) { // always do full echo during startup + uint8_t nv_type; + do { + if ((nv_type = nv_get_type(nv)) == NV_TYPE_0) break; + + if (nv_type == NV_TYPE_GCODE) { + if (js.echo_json_gcode_block == false) { // kill command echo if not enabled + nv->valuetype = TYPE_EMPTY; + } + +//++++ } else if (nv_type == NV_TYPE_CONFIG) { // kill config echo if not enabled +//fix me if (js.echo_json_configs == false) { +// nv->valuetype = TYPE_EMPTY; +// } + + } else if (nv_type == NV_TYPE_MESSAGE) { // kill message echo if not enabled + if (js.echo_json_messages == false) { + nv->valuetype = TYPE_EMPTY; + } + + } else if (nv_type == NV_TYPE_LINENUM) { // kill line number echo if not enabled + if ((js.echo_json_linenum == false) || (fp_ZERO(nv->value))) { // do not report line# 0 + nv->valuetype = TYPE_EMPTY; + } + } + } while ((nv = nv->nx) != 0); + } + + // Footer processing + while(nv->valuetype != TYPE_EMPTY) { // find a free nvObj at end of the list... + if ((nv = nv->nx) == 0) { //...or hit the 0 and return w/o a footer + json_serialize(nv_header, cs.out_buf, sizeof(cs.out_buf)); + return; + } + } + char_t footer_string[NV_FOOTER_LEN]; + sprintf((char *)footer_string, "%d,%d,%d,0", FOOTER_REVISION, status, cs.linelen); + cs.linelen = 0; // reset linelen so it's only reported once + + nv_copy_string(nv, footer_string); // link string to nv object +// nv->depth = 0; // footer 'f' is a peer to response 'r' (hard wired to 0) + nv->depth = js.json_footer_depth; // 0=footer is peer to response 'r', 1=child of response 'r' + nv->valuetype = TYPE_ARRAY; + strcpy(nv->token, "f"); // terminate the list + nv->nx = 0; + + // do all this to avoid having to serialize it twice + int16_t strcount = json_serialize(nv_header, cs.out_buf, sizeof(cs.out_buf));// make JSON string w/o checksum + if (strcount < 0) { return;} // encountered an overrun during serialization + if (strcount > OUTPUT_BUFFER_LEN - MAX_TAIL_LEN) { return;} // would overrun during checksum generation + int16_t strcount2 = strcount; + char tail[MAX_TAIL_LEN]; + + while (cs.out_buf[strcount] != '0') { strcount--; } // find end of checksum + strcpy(tail, cs.out_buf + strcount + 1); // save the json termination + + while (cs.out_buf[strcount2] != ',') { strcount2--; }// find start of checksum + sprintf((char *)cs.out_buf + strcount2 + 1, "%d%s", compute_checksum(cs.out_buf, strcount2), tail); + fprintf(stderr, "%s", cs.out_buf); } /*********************************************************************************** @@ -555,23 +555,23 @@ void json_print_response(uint8_t status) stat_t json_set_jv(nvObj_t *nv) { - if (nv->value > JV_VERBOSE) - return (STAT_INPUT_VALUE_RANGE_ERROR); - js.json_verbosity = nv->value; - - js.echo_json_footer = false; - js.echo_json_messages = false; - js.echo_json_configs = false; - js.echo_json_linenum = false; - js.echo_json_gcode_block = false; - - if (nv->value >= JV_FOOTER) { js.echo_json_footer = true;} - if (nv->value >= JV_MESSAGES) { js.echo_json_messages = true;} - if (nv->value >= JV_CONFIGS) { js.echo_json_configs = true;} - if (nv->value >= JV_LINENUM) { js.echo_json_linenum = true;} - if (nv->value >= JV_VERBOSE) { js.echo_json_gcode_block = true;} - - return(STAT_OK); + if (nv->value > JV_VERBOSE) + return STAT_INPUT_VALUE_RANGE_ERROR; + js.json_verbosity = nv->value; + + js.echo_json_footer = false; + js.echo_json_messages = false; + js.echo_json_configs = false; + js.echo_json_linenum = false; + js.echo_json_gcode_block = false; + + if (nv->value >= JV_FOOTER) { js.echo_json_footer = true;} + if (nv->value >= JV_MESSAGES) { js.echo_json_messages = true;} + if (nv->value >= JV_CONFIGS) { js.echo_json_configs = true;} + if (nv->value >= JV_LINENUM) { js.echo_json_linenum = true;} + if (nv->value >= JV_VERBOSE) { js.echo_json_gcode_block = true;} + + return(STAT_OK); } diff --git a/src/json_parser.h b/src/json_parser.h index 9b73f06..cc524b7 100755 --- a/src/json_parser.h +++ b/src/json_parser.h @@ -39,40 +39,40 @@ #define JSON_OUTPUT_STRING_MAX (OUTPUT_BUFFER_LEN) enum jsonVerbosity { - JV_SILENT = 0, // no response is provided for any command - JV_FOOTER, // returns footer only (no command echo, gcode blocks or messages) - JV_MESSAGES, // returns footer, messages (exception and gcode messages) - JV_CONFIGS, // returns footer, messages, config commands - JV_LINENUM, // returns footer, messages, config commands, gcode line numbers if present - JV_VERBOSE // returns footer, messages, config commands, gcode blocks + JV_SILENT = 0, // no response is provided for any command + JV_FOOTER, // returns footer only (no command echo, gcode blocks or messages) + JV_MESSAGES, // returns footer, messages (exception and gcode messages) + JV_CONFIGS, // returns footer, messages, config commands + JV_LINENUM, // returns footer, messages, config commands, gcode line numbers if present + JV_VERBOSE // returns footer, messages, config commands, gcode blocks }; -enum jsonFormats { // json output print modes - JSON_NO_PRINT = 0, // don't print anything if you find yourself in JSON mode - JSON_OBJECT_FORMAT, // print just the body as a json object - JSON_RESPONSE_FORMAT // print the header/body/footer as a response object +enum jsonFormats { // json output print modes + JSON_NO_PRINT = 0, // don't print anything if you find yourself in JSON mode + JSON_OBJECT_FORMAT, // print just the body as a json object + JSON_RESPONSE_FORMAT // print the header/body/footer as a response object }; enum jsonSyntaxMode { - JSON_SYNTAX_RELAXED = 0, // Does not require quotes on names - JSON_SYNTAX_STRICT // requires quotes on names + JSON_SYNTAX_RELAXED = 0, // Does not require quotes on names + JSON_SYNTAX_STRICT // requires quotes on names }; typedef struct jsSingleton { - /*** config values (PUBLIC) ***/ - uint8_t json_verbosity; // see enum in this file for settings - uint8_t json_footer_depth; // 0=footer is peer to response 'r', 1=child of response 'r' -// uint8_t json_footer_style; // select footer style - uint8_t json_syntax; // 0=relaxed syntax, 1=strict syntax + /*** config values (PUBLIC) ***/ + uint8_t json_verbosity; // see enum in this file for settings + uint8_t json_footer_depth; // 0=footer is peer to response 'r', 1=child of response 'r' +// uint8_t json_footer_style; // select footer style + uint8_t json_syntax; // 0=relaxed syntax, 1=strict syntax - uint8_t echo_json_footer; // flags for JSON responses serialization - uint8_t echo_json_messages; - uint8_t echo_json_configs; - uint8_t echo_json_linenum; - uint8_t echo_json_gcode_block; + uint8_t echo_json_footer; // flags for JSON responses serialization + uint8_t echo_json_messages; + uint8_t echo_json_configs; + uint8_t echo_json_linenum; + uint8_t echo_json_gcode_block; - /*** runtime values (PRIVATE) ***/ + /*** runtime values (PRIVATE) ***/ } jsSingleton_t; @@ -92,17 +92,17 @@ stat_t json_set_jv(nvObj_t *nv); #ifdef __TEXT_MODE - void js_print_ej(nvObj_t *nv); - void js_print_jv(nvObj_t *nv); - void js_print_js(nvObj_t *nv); - void js_print_fs(nvObj_t *nv); + void js_print_ej(nvObj_t *nv); + void js_print_jv(nvObj_t *nv); + void js_print_js(nvObj_t *nv); + void js_print_fs(nvObj_t *nv); #else - #define js_print_ej tx_print_stub - #define js_print_jv tx_print_stub - #define js_print_js tx_print_stub - #define js_print_fs tx_print_stub + #define js_print_ej tx_print_stub + #define js_print_jv tx_print_stub + #define js_print_js tx_print_stub + #define js_print_fs tx_print_stub #endif // __TEXT_MODE diff --git a/src/kinematics.c b/src/kinematics.c index 806d892..622c5b5 100755 --- a/src/kinematics.c +++ b/src/kinematics.c @@ -35,37 +35,37 @@ /* * ik_kinematics() - wrapper routine for inverse kinematics * - * Calls kinematics function(s). - * Performs axis mapping & conversion of length units to steps (and deals with inhibited axes) + * Calls kinematics function(s). + * Performs axis mapping & conversion of length units to steps (and deals with inhibited axes) * - * The reason steps are returned as floats (as opposed to, say, uint32_t) is to accommodate - * fractional DDA steps. The DDA deals with fractional step values as fixed-point binary in - * order to get the smoothest possible operation. Steps are passed to the move prep routine - * as floats and converted to fixed-point binary during queue loading. See stepper.c for details. + * The reason steps are returned as floats (as opposed to, say, uint32_t) is to accommodate + * fractional DDA steps. The DDA deals with fractional step values as fixed-point binary in + * order to get the smoothest possible operation. Steps are passed to the move prep routine + * as floats and converted to fixed-point binary during queue loading. See stepper.c for details. */ void ik_kinematics(const float travel[], float steps[]) { - float joint[AXES]; + float joint[AXES]; -// _inverse_kinematics(travel, joint); // you can insert inverse kinematics transformations here - memcpy(joint, travel, sizeof(float)*AXES); //...or just do a memcpy for Cartesian machines +// _inverse_kinematics(travel, joint); // you can insert inverse kinematics transformations here + memcpy(joint, travel, sizeof(float)*AXES); //...or just do a memcpy for Cartesian machines - // Map motors to axes and convert length units to steps - // Most of the conversion math has already been done in during config in steps_per_unit() - // which takes axis travel, step angle and microsteps into account. - for (uint8_t axis=0; axis= 5) - if (st_cfg.mot[MOTOR_5].motor_map == axis) { steps[MOTOR_5] = joint[axis] * st_cfg.mot[MOTOR_5].steps_per_unit;} + if (st_cfg.mot[MOTOR_5].motor_map == axis) { steps[MOTOR_5] = joint[axis] * st_cfg.mot[MOTOR_5].steps_per_unit;} #endif #if (MOTORS >= 6) - if (st_cfg.mot[MOTOR_6].motor_map == axis) { steps[MOTOR_6] = joint[axis] * st_cfg.mot[MOTOR_6].steps_per_unit;} + if (st_cfg.mot[MOTOR_6].motor_map == axis) { steps[MOTOR_6] = joint[axis] * st_cfg.mot[MOTOR_6].steps_per_unit;} #endif - } + } } diff --git a/src/main.c b/src/main.c index df39d12..2d6a379 100755 --- a/src/main.c +++ b/src/main.c @@ -17,11 +17,9 @@ * 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. */ -/* See github.com/Synthetos/tinyg for code and docs on the wiki - */ -#include "tinyg.h" // #1 There are some dependencies -#include "config.h" // #2 +#include "tinyg.h" // #1 There are some dependencies +#include "config.h" // #2 #include "hardware.h" #include "persistence.h" #include "controller.h" @@ -34,391 +32,53 @@ #include "switch.h" #include "test.h" #include "pwm.h" -#include "xio.h" +#include "xio/xio.h" #include #include "xmega/xmega_interrupts.h" -void _init() __attribute__ ((weak)); -void _init() {;} - -void __libc_init_array(void); -/******************** Application Code ************************/ -static void _application_init(void) -{ - // There are a lot of dependencies in the order of these inits. - // Don't change the ordering unless you understand this. +static void init() { + // There are a lot of dependencies in the order of these inits. + // Don't change the ordering unless you understand this. - cli(); + cli(); - // do these first - hardware_init(); // system hardware setup - must be first - persistence_init(); // set up EEPROM or other NVM - must be second - rtc_init(); // real time counter - xio_init(); // eXtended IO subsystem + // do these first + hardware_init(); // system hardware setup - must be first + persistence_init(); // set up EEPROM or other NVM - must be second + rtc_init(); // real time counter + xio_init(); // eXtended IO subsystem - // do these next - stepper_init(); // stepper subsystem - must precede gpio_init() - encoder_init(); // virtual encoders - switch_init(); // switches -// gpio_init(); // parallel IO - pwm_init(); // pulse width modulation drivers - must follow gpio_init() + // do these next + stepper_init(); // stepper subsystem + encoder_init(); // virtual encoders + switch_init(); // switches + pwm_init(); // pulse width modulation drivers - controller_init(STD_IN, STD_OUT, STD_ERR);// must be first app init; reqs xio_init() - config_init(); // config records from eeprom - must be next app init - network_init(); // reset std devices if required - must follow config_init() - planner_init(); // motion planning subsystem - canonical_machine_init(); // canonical machine - must follow config_init() + controller_init(STD_IN, STD_OUT, STD_ERR);// must be first app init; reqs xio_init() + config_init(); // config records from eeprom - must be next app init + network_init(); // reset std devices if required - must follow config_init() + planner_init(); // motion planning subsystem + canonical_machine_init(); // canonical machine - must follow config_init() - // now bring up the interrupts and get started - PMIC_SetVectorLocationToApplication();// as opposed to boot ROM - PMIC_EnableHighLevel(); // all levels are used, so don't bother to abstract them - PMIC_EnableMediumLevel(); - PMIC_EnableLowLevel(); - sei(); // enable global interrupts - rpt_print_system_ready_message();// (LAST) announce system is ready -} + // now bring up the interrupts and get started + PMIC_SetVectorLocationToApplication();// as opposed to boot ROM + PMIC_EnableHighLevel(); // all levels are used, so don't bother to abstract them + PMIC_EnableMediumLevel(); + PMIC_EnableLowLevel(); -/* - * main() - */ + sei(); // enable global interrupts -int main(void) -{ - // TinyG application setup - _application_init(); - run_canned_startup(); // run any pre-loaded commands - - // main loop - for (;;) { - controller_run( ); // single pass through the controller - } - return 0; + rpt_print_system_ready_message();// (LAST) announce system is ready } -/**** Status Messages *************************************************************** - * get_status_message() - return the status message - * - * See tinyg.h for status codes. These strings must align with the status codes in tinyg.h - * The number of elements in the indexing array must match the # of strings - * - * Reference for putting display strings and string arrays in AVR program memory: - * http://www.cs.mun.ca/~paul/cs4723/material/atmel/avr-libc-user-manual-1.6.5/pgmspace.html - */ - -stat_t status_code; // allocate a variable for the ritorno macro -char global_string_buf[MESSAGE_LEN]; // allocate a string for global message use - -//#ifdef __TEXT_MODE - -/*** Status message strings ***/ - -static const char stat_00[] PROGMEM = "OK"; -static const char stat_01[] PROGMEM = "Error"; -static const char stat_02[] PROGMEM = "Eagain"; -static const char stat_03[] PROGMEM = "Noop"; -static const char stat_04[] PROGMEM = "Complete"; -static const char stat_05[] PROGMEM = "Terminated"; -static const char stat_06[] PROGMEM = "Hard reset"; -static const char stat_07[] PROGMEM = "End of line"; -static const char stat_08[] PROGMEM = "End of file"; -static const char stat_09[] PROGMEM = "File not open"; - -static const char stat_10[] PROGMEM = "Max file size exceeded"; -static const char stat_11[] PROGMEM = "No such device"; -static const char stat_12[] PROGMEM = "Buffer empty"; -static const char stat_13[] PROGMEM = "Buffer full"; -static const char stat_14[] PROGMEM = "Buffer full - fatal"; -static const char stat_15[] PROGMEM = "Initializing"; -static const char stat_16[] PROGMEM = "Entering boot loader"; -static const char stat_17[] PROGMEM = "Function is stubbed"; -static const char stat_18[] PROGMEM = "18"; -static const char stat_19[] PROGMEM = "19"; - -static const char stat_20[] PROGMEM = "Internal error"; -static const char stat_21[] PROGMEM = "Internal range error"; -static const char stat_22[] PROGMEM = "Floating point error"; -static const char stat_23[] PROGMEM = "Divide by zero"; -static const char stat_24[] PROGMEM = "Invalid Address"; -static const char stat_25[] PROGMEM = "Read-only address"; -static const char stat_26[] PROGMEM = "Initialization failure"; -static const char stat_27[] PROGMEM = "System alarm - shutting down"; -static const char stat_28[] PROGMEM = "Failed to get planner buffer"; -static const char stat_29[] PROGMEM = "Generic exception report"; - -static const char stat_30[] PROGMEM = "Move time is infinite"; -static const char stat_31[] PROGMEM = "Move time is NAN"; -static const char stat_32[] PROGMEM = "Float is infinite"; -static const char stat_33[] PROGMEM = "Float is NAN"; -static const char stat_34[] PROGMEM = "Persistence error"; -static const char stat_35[] PROGMEM = "Bad status report setting"; -static const char stat_36[] PROGMEM = "36"; -static const char stat_37[] PROGMEM = "37"; -static const char stat_38[] PROGMEM = "38"; -static const char stat_39[] PROGMEM = "39"; - -static const char stat_40[] PROGMEM = "40"; -static const char stat_41[] PROGMEM = "41"; -static const char stat_42[] PROGMEM = "42"; -static const char stat_43[] PROGMEM = "43"; -static const char stat_44[] PROGMEM = "44"; -static const char stat_45[] PROGMEM = "45"; -static const char stat_46[] PROGMEM = "46"; -static const char stat_47[] PROGMEM = "47"; -static const char stat_48[] PROGMEM = "48"; -static const char stat_49[] PROGMEM = "49"; -static const char stat_50[] PROGMEM = "50"; -static const char stat_51[] PROGMEM = "51"; -static const char stat_52[] PROGMEM = "52"; -static const char stat_53[] PROGMEM = "53"; -static const char stat_54[] PROGMEM = "54"; -static const char stat_55[] PROGMEM = "55"; -static const char stat_56[] PROGMEM = "56"; -static const char stat_57[] PROGMEM = "57"; -static const char stat_58[] PROGMEM = "58"; -static const char stat_59[] PROGMEM = "59"; -static const char stat_60[] PROGMEM = "60"; -static const char stat_61[] PROGMEM = "61"; -static const char stat_62[] PROGMEM = "62"; -static const char stat_63[] PROGMEM = "63"; -static const char stat_64[] PROGMEM = "64"; -static const char stat_65[] PROGMEM = "65"; -static const char stat_66[] PROGMEM = "66"; -static const char stat_67[] PROGMEM = "67"; -static const char stat_68[] PROGMEM = "68"; -static const char stat_69[] PROGMEM = "69"; -static const char stat_70[] PROGMEM = "70"; -static const char stat_71[] PROGMEM = "71"; -static const char stat_72[] PROGMEM = "72"; -static const char stat_73[] PROGMEM = "73"; -static const char stat_74[] PROGMEM = "74"; -static const char stat_75[] PROGMEM = "75"; -static const char stat_76[] PROGMEM = "76"; -static const char stat_77[] PROGMEM = "77"; -static const char stat_78[] PROGMEM = "78"; -static const char stat_79[] PROGMEM = "79"; -static const char stat_80[] PROGMEM = "80"; -static const char stat_81[] PROGMEM = "81"; -static const char stat_82[] PROGMEM = "82"; -static const char stat_83[] PROGMEM = "83"; -static const char stat_84[] PROGMEM = "84"; -static const char stat_85[] PROGMEM = "85"; -static const char stat_86[] PROGMEM = "86"; -static const char stat_87[] PROGMEM = "87"; -static const char stat_88[] PROGMEM = "88"; -static const char stat_89[] PROGMEM = "89"; - -static const char stat_90[] PROGMEM = "Config sub-system assertion failure"; -static const char stat_91[] PROGMEM = "IO sub-system assertion failure"; -static const char stat_92[] PROGMEM = "Encoder assertion failure"; -static const char stat_93[] PROGMEM = "Stepper assertion failure"; -static const char stat_94[] PROGMEM = "Planner assertion failure"; -static const char stat_95[] PROGMEM = "Canonical machine assertion failure"; -static const char stat_96[] PROGMEM = "Controller assertion failure"; -static const char stat_97[] PROGMEM = "Stack overflow detected"; -static const char stat_98[] PROGMEM = "Memory fault detected"; -static const char stat_99[] PROGMEM = "Generic assertion failure"; - -static const char stat_100[] PROGMEM = "Unrecognized command or config name"; -static const char stat_101[] PROGMEM = "Invalid or malformed command"; -static const char stat_102[] PROGMEM = "Bad number format"; -static const char stat_103[] PROGMEM = "Unsupported number or JSON type"; -static const char stat_104[] PROGMEM = "Parameter is read-only"; -static const char stat_105[] PROGMEM = "Parameter cannot be read"; -static const char stat_106[] PROGMEM = "Command not accepted at this time"; -static const char stat_107[] PROGMEM = "Input exceeds max length"; -static const char stat_108[] PROGMEM = "Input less than minimum value"; -static const char stat_109[] PROGMEM = "Input exceeds maximum value"; - -static const char stat_110[] PROGMEM = "Input value range error"; -static const char stat_111[] PROGMEM = "JSON syntax error"; -static const char stat_112[] PROGMEM = "JSON input has too many pairs"; -static const char stat_113[] PROGMEM = "JSON string too long"; -static const char stat_114[] PROGMEM = "114"; -static const char stat_115[] PROGMEM = "115"; -static const char stat_116[] PROGMEM = "116"; -static const char stat_117[] PROGMEM = "117"; -static const char stat_118[] PROGMEM = "118"; -static const char stat_119[] PROGMEM = "119"; - -static const char stat_120[] PROGMEM = "120"; -static const char stat_121[] PROGMEM = "121"; -static const char stat_122[] PROGMEM = "122"; -static const char stat_123[] PROGMEM = "123"; -static const char stat_124[] PROGMEM = "124"; -static const char stat_125[] PROGMEM = "125"; -static const char stat_126[] PROGMEM = "126"; -static const char stat_127[] PROGMEM = "127"; -static const char stat_128[] PROGMEM = "128"; -static const char stat_129[] PROGMEM = "129"; - -static const char stat_130[] PROGMEM = "Generic Gcode input error"; -static const char stat_131[] PROGMEM = "Gcode command unsupported"; -static const char stat_132[] PROGMEM = "M code unsupported"; -static const char stat_133[] PROGMEM = "Gcode modal group violation"; -static const char stat_134[] PROGMEM = "Axis word missing"; -static const char stat_135[] PROGMEM = "Axis cannot be present"; -static const char stat_136[] PROGMEM = "Axis is invalid for this command"; -static const char stat_137[] PROGMEM = "Axis is disabled"; -static const char stat_138[] PROGMEM = "Axis target position is missing"; -static const char stat_139[] PROGMEM = "Axis target position is invalid"; - -static const char stat_140[] PROGMEM = "Selected plane is missing"; -static const char stat_141[] PROGMEM = "Selected plane is invalid"; -static const char stat_142[] PROGMEM = "Feedrate not specified"; -static const char stat_143[] PROGMEM = "Inverse time mode cannot be used with this command"; -static const char stat_144[] PROGMEM = "Rotary axes cannot be used with this command"; -static const char stat_145[] PROGMEM = "G0 or G1 must be active for G53"; -static const char stat_146[] PROGMEM = "Requested velocity exceeds limits"; -static const char stat_147[] PROGMEM = "Cutter compensation cannot be enabled"; -static const char stat_148[] PROGMEM = "Programmed point same as current point"; -static const char stat_149[] PROGMEM = "Spindle speed below minimum"; - -static const char stat_150[] PROGMEM = "Spindle speed exceeded maximum"; -static const char stat_151[] PROGMEM = "Spindle S word is missing"; -static const char stat_152[] PROGMEM = "Spindle S word is invalid"; -static const char stat_153[] PROGMEM = "Spindle must be off for this command"; -static const char stat_154[] PROGMEM = "Spindle must be turning for this command"; -static const char stat_155[] PROGMEM = "Arc specification error"; -static const char stat_156[] PROGMEM = "Arc specification error - missing axis(es)"; -static const char stat_157[] PROGMEM = "Arc specification error - missing offset(s)"; -static const char stat_158[] PROGMEM = "Arc specification error - radius arc out of tolerance"; // current longest message: 56 chard -static const char stat_159[] PROGMEM = "Arc specification error - endpoint is starting point"; - -static const char stat_160[] PROGMEM = "P word is missing"; -static const char stat_161[] PROGMEM = "P word is invalid"; -static const char stat_162[] PROGMEM = "P word is zero"; -static const char stat_163[] PROGMEM = "P word is negative"; -static const char stat_164[] PROGMEM = "P word is not an integer"; -static const char stat_165[] PROGMEM = "P word is not a valid tool number"; -static const char stat_166[] PROGMEM = "D word is missing"; -static const char stat_167[] PROGMEM = "D word is invalid"; -static const char stat_168[] PROGMEM = "E word is missing"; -static const char stat_169[] PROGMEM = "E word is invalid"; - -static const char stat_170[] PROGMEM = "H word is missing"; -static const char stat_171[] PROGMEM = "H word is invalid"; -static const char stat_172[] PROGMEM = "L word is missing"; -static const char stat_173[] PROGMEM = "L word is invalid"; -static const char stat_174[] PROGMEM = "Q word is missing"; -static const char stat_175[] PROGMEM = "Q word is invalid"; -static const char stat_176[] PROGMEM = "R word is missing"; -static const char stat_177[] PROGMEM = "R word is invalid"; -static const char stat_178[] PROGMEM = "T word is missing"; -static const char stat_179[] PROGMEM = "T word is invalid"; - -static const char stat_180[] PROGMEM = "180"; -static const char stat_181[] PROGMEM = "181"; -static const char stat_182[] PROGMEM = "182"; -static const char stat_183[] PROGMEM = "183"; -static const char stat_184[] PROGMEM = "184"; -static const char stat_185[] PROGMEM = "185"; -static const char stat_186[] PROGMEM = "186"; -static const char stat_187[] PROGMEM = "187"; -static const char stat_188[] PROGMEM = "188"; -static const char stat_189[] PROGMEM = "189"; - -static const char stat_190[] PROGMEM = "190"; -static const char stat_191[] PROGMEM = "191"; -static const char stat_192[] PROGMEM = "192"; -static const char stat_193[] PROGMEM = "193"; -static const char stat_194[] PROGMEM = "194"; -static const char stat_195[] PROGMEM = "195"; -static const char stat_196[] PROGMEM = "196"; -static const char stat_197[] PROGMEM = "197"; -static const char stat_198[] PROGMEM = "198"; -static const char stat_199[] PROGMEM = "199"; - -static const char stat_200[] PROGMEM = "Generic TinyG error"; -static const char stat_201[] PROGMEM = "Move less than minimum length"; -static const char stat_202[] PROGMEM = "Move less than minimum time"; -static const char stat_203[] PROGMEM = "Machine is alarmed - Command not processed"; // current longest message 43 chars (including NUL) -static const char stat_204[] PROGMEM = "Limit switch hit - Shutdown occurred"; -static const char stat_205[] PROGMEM = "Trapezoid planner failed to converge"; -static const char stat_206[] PROGMEM = "206"; -static const char stat_207[] PROGMEM = "207"; -static const char stat_208[] PROGMEM = "208"; -static const char stat_209[] PROGMEM = "209"; - -static const char stat_210[] PROGMEM = "210"; -static const char stat_211[] PROGMEM = "211"; -static const char stat_212[] PROGMEM = "212"; -static const char stat_213[] PROGMEM = "213"; -static const char stat_214[] PROGMEM = "214"; -static const char stat_215[] PROGMEM = "215"; -static const char stat_216[] PROGMEM = "216"; -static const char stat_217[] PROGMEM = "217"; -static const char stat_218[] PROGMEM = "218"; -static const char stat_219[] PROGMEM = "219"; - -static const char stat_220[] PROGMEM = "Soft limit exceeded"; -static const char stat_221[] PROGMEM = "Soft limit exceeded - X min"; -static const char stat_222[] PROGMEM = "Soft limit exceeded - X max"; -static const char stat_223[] PROGMEM = "Soft limit exceeded - Y min"; -static const char stat_224[] PROGMEM = "Soft limit exceeded - Y max"; -static const char stat_225[] PROGMEM = "Soft limit exceeded - Z min"; -static const char stat_226[] PROGMEM = "Soft limit exceeded - Z max"; -static const char stat_227[] PROGMEM = "Soft limit exceeded - A min"; -static const char stat_228[] PROGMEM = "Soft limit exceeded - A max"; -static const char stat_229[] PROGMEM = "Soft limit exceeded - B min"; -static const char stat_230[] PROGMEM = "Soft limit exceeded - B max"; -static const char stat_231[] PROGMEM = "Soft limit exceeded - C min"; -static const char stat_232[] PROGMEM = "Soft limit exceeded - C max"; -static const char stat_233[] PROGMEM = "233"; -static const char stat_234[] PROGMEM = "234"; -static const char stat_235[] PROGMEM = "235"; -static const char stat_236[] PROGMEM = "236"; -static const char stat_237[] PROGMEM = "237"; -static const char stat_238[] PROGMEM = "238"; -static const char stat_239[] PROGMEM = "239"; - -static const char stat_240[] PROGMEM = "Homing cycle failed"; -static const char stat_241[] PROGMEM = "Homing Error - Bad or no axis specified"; -static const char stat_242[] PROGMEM = "Homing Error - Search velocity is zero"; -static const char stat_243[] PROGMEM = "Homing Error - Latch velocity is zero"; -static const char stat_244[] PROGMEM = "Homing Error - Travel min & max are the same"; -static const char stat_245[] PROGMEM = "Homing Error - Negative latch backoff"; -static const char stat_246[] PROGMEM = "Homing Error - Homing switches misconfigured"; -static const char stat_247[] PROGMEM = "247"; -static const char stat_248[] PROGMEM = "248"; -static const char stat_249[] PROGMEM = "249"; - -static const char stat_250[] PROGMEM = "Probe cycle failed"; -static const char stat_251[] PROGMEM = "Probe endpoint is starting point"; -static const char stat_252[] PROGMEM = "Jogging cycle failed"; +int main() { + init(); -static const char *const stat_msg[] PROGMEM = { - stat_00, stat_01, stat_02, stat_03, stat_04, stat_05, stat_06, stat_07, stat_08, stat_09, - stat_10, stat_11, stat_12, stat_13, stat_14, stat_15, stat_16, stat_17, stat_18, stat_19, - stat_20, stat_21, stat_22, stat_23, stat_24, stat_25, stat_26, stat_27, stat_28, stat_29, - stat_30, stat_31, stat_32, stat_33, stat_34, stat_35, stat_36, stat_37, stat_38, stat_39, - stat_40, stat_41, stat_42, stat_43, stat_44, stat_45, stat_46, stat_47, stat_48, stat_49, - stat_50, stat_51, stat_52, stat_53, stat_54, stat_55, stat_56, stat_57, stat_58, stat_59, - stat_60, stat_61, stat_62, stat_63, stat_64, stat_65, stat_66, stat_67, stat_68, stat_69, - stat_70, stat_71, stat_72, stat_73, stat_74, stat_75, stat_76, stat_77, stat_78, stat_79, - stat_80, stat_81, stat_82, stat_83, stat_84, stat_85, stat_86, stat_87, stat_88, stat_89, - stat_90, stat_91, stat_92, stat_93, stat_94, stat_95, stat_96, stat_97, stat_98, stat_99, - stat_100, stat_101, stat_102, stat_103, stat_104, stat_105, stat_106, stat_107, stat_108, stat_109, - stat_110, stat_111, stat_112, stat_113, stat_114, stat_115, stat_116, stat_117, stat_118, stat_119, - stat_120, stat_121, stat_122, stat_123, stat_124, stat_125, stat_126, stat_127, stat_128, stat_129, - stat_130, stat_131, stat_132, stat_133, stat_134, stat_135, stat_136, stat_137, stat_138, stat_139, - stat_140, stat_141, stat_142, stat_143, stat_144, stat_145, stat_146, stat_147, stat_148, stat_149, - stat_150, stat_151, stat_152, stat_153, stat_154, stat_155, stat_156, stat_157, stat_158, stat_159, - stat_160, stat_161, stat_162, stat_163, stat_164, stat_165, stat_166, stat_167, stat_168, stat_169, - stat_170, stat_171, stat_172, stat_173, stat_174, stat_175, stat_176, stat_177, stat_178, stat_179, - stat_180, stat_181, stat_182, stat_183, stat_184, stat_185, stat_186, stat_187, stat_188, stat_189, - stat_190, stat_191, stat_192, stat_193, stat_194, stat_195, stat_196, stat_197, stat_198, stat_199, - stat_200, stat_201, stat_202, stat_203, stat_204, stat_205, stat_206, stat_207, stat_208, stat_209, - stat_210, stat_211, stat_212, stat_213, stat_214, stat_215, stat_216, stat_217, stat_218, stat_219, - stat_220, stat_221, stat_222, stat_223, stat_224, stat_225, stat_226, stat_227, stat_228, stat_229, - stat_230, stat_231, stat_232, stat_233, stat_234, stat_235, stat_236, stat_237, stat_238, stat_239, - stat_240, stat_241, stat_242, stat_243, stat_244, stat_245, stat_246, stat_247, stat_248, stat_249, - stat_250, stat_251, stat_252 -}; + // main loop + while (true) controller_run(); // single pass through the controller -char *get_status_message(stat_t status) -{ - return ((char *)GET_TEXT_ITEM(stat_msg, status)); + return 0; } diff --git a/src/network.c b/src/network.c index 982eb87..ac2ba7d 100755 --- a/src/network.c +++ b/src/network.c @@ -24,21 +24,21 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* This module is really nothing mre than a placeholder at this time. - * "Networking" refers to a planned RS485 broadcast network to support - * multi-board configs and external RS485 devices such as extruders. - * Basic operation of RS485 on the TinyG hardware has been verified - * using what's in this file, but you won;t find much more. +/* This module is really nothing mre than a placeholder at this time. + * "Networking" refers to a planned RS485 broadcast network to support + * multi-board configs and external RS485 devices such as extruders. + * Basic operation of RS485 on the TinyG hardware has been verified + * using what's in this file, but you won;t find much more. */ -#include // for tests +#include // for tests #include "tinyg.h" #include "network.h" #include "controller.h" #include "gpio.h" #include "hardware.h" -#include "xio.h" +#include "xio/xio.h" /* * Local Scope Functions and Data @@ -49,17 +49,17 @@ */ void network_init() { - // re-point IO if in slave mode - if (cs.network_mode == NETWORK_SLAVE) { - controller_init(XIO_DEV_RS485, XIO_DEV_USB, XIO_DEV_USB); - tg_set_secondary_source(XIO_DEV_USB); - } - xio_enable_rs485_rx(); // needed for clean start for RS-485; + // re-point IO if in slave mode + if (cs.network_mode == NETWORK_SLAVE) { + controller_init(XIO_DEV_RS485, XIO_DEV_USB, XIO_DEV_USB); + tg_set_secondary_source(XIO_DEV_USB); + } + xio_enable_rs485_rx(); // needed for clean start for RS-485; } void net_forward(unsigned char c) { - xio_putc(XIO_DEV_RS485, c); // write to RS485 port + xio_putc(XIO_DEV_RS485, c); // write to RS485 port } /* @@ -69,46 +69,46 @@ void net_forward(unsigned char c) uint8_t net_test_rxtx(uint8_t c) { - int d; + int d; - // master operation - if (cs.network_mode == NETWORK_MASTER) { - if ((c < 0x20) || (c >= 0x7F)) { c = 0x20; } - c++; - xio_putc(XIO_DEV_RS485, c); // write to RS485 port - xio_putc(XIO_DEV_USB, c); // write to USB port - _delay_ms(2); + // master operation + if (cs.network_mode == NETWORK_MASTER) { + if ((c < 0x20) || (c >= 0x7F)) { c = 0x20; } + c++; + xio_putc(XIO_DEV_RS485, c); // write to RS485 port + xio_putc(XIO_DEV_USB, c); // write to USB port + _delay_ms(2); - // slave operation - } else { - if ((d = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { - xio_putc(XIO_DEV_USB, d); - } - } - return (c); + // slave operation + } else { + if ((d = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { + xio_putc(XIO_DEV_USB, d); + } + } + return c; } uint8_t net_test_loopback(uint8_t c) { - if (cs.network_mode == NETWORK_MASTER) { - // send a character - if ((c < 0x20) || (c >= 0x7F)) { c = 0x20; } - c++; - xio_putc(XIO_DEV_RS485, c); // write to RS485 port + if (cs.network_mode == NETWORK_MASTER) { + // send a character + if ((c < 0x20) || (c >= 0x7F)) { c = 0x20; } + c++; + xio_putc(XIO_DEV_RS485, c); // write to RS485 port - // wait for loopback character - while (true) { - if ((c = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { - xio_putc(XIO_DEV_USB, c); // write to USB port - } - } - } else { - if ((c = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { - xio_putc(XIO_DEV_RS485, c); // write back to master - xio_putc(XIO_DEV_USB, c); // write to slave USB - } - } - _delay_ms(2); - return (c); + // wait for loopback character + while (true) { + if ((c = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { + xio_putc(XIO_DEV_USB, c); // write to USB port + } + } + } else { + if ((c = xio_getc(XIO_DEV_RS485)) != _FDEV_ERR) { + xio_putc(XIO_DEV_RS485, c); // write back to master + xio_putc(XIO_DEV_USB, c); // write to slave USB + } + } + _delay_ms(2); + return c; } diff --git a/src/network.h b/src/network.h index b054e6d..9ef2c4f 100755 --- a/src/network.h +++ b/src/network.h @@ -32,9 +32,9 @@ */ enum networkMode { - NETWORK_STANDALONE = 0, - NETWORK_MASTER, - NETWORK_SLAVE + NETWORK_STANDALONE = 0, + NETWORK_MASTER, + NETWORK_SLAVE }; void network_init(); @@ -42,7 +42,7 @@ void net_forward(unsigned char c); uint8_t net_test_rxtx(uint8_t c); uint8_t net_test_loopback(uint8_t c); -#define XIO_DEV_NET XIO_DEV_RS485 // define the network channel +#define XIO_DEV_NET XIO_DEV_RS485 // define the network channel //#define net_forward(c) (xio_putc(XIO_DEV_NET,c)) #endif diff --git a/src/persistence.c b/src/persistence.c index 10471f9..7a3f154 100755 --- a/src/persistence.c +++ b/src/persistence.c @@ -34,39 +34,39 @@ nvmSingleton_t nvm; -void persistence_init() -{ - nvm.base_addr = NVM_BASE_ADDR; - nvm.profile_base = 0; - return; +void persistence_init() { + nvm.base_addr = NVM_BASE_ADDR; + nvm.profile_base = 0; + return; } + /************************************************************************************ - * read_persistent_value() - return value (as float) by index + * read_persistent_value() - return value (as float) by index * write_persistent_value() - write to NVM by index, but only if the value has changed * - * It's the responsibility of the caller to make sure the index does not exceed range + * It's the responsibility of the caller to make sure the index does not exceed range */ - -stat_t read_persistent_value(nvObj_t *nv) -{ - nvm.address = nvm.profile_base + (nv->index * NVM_VALUE_LEN); - (void)EEPROM_ReadBytes(nvm.address, nvm.byte_array, NVM_VALUE_LEN); - memcpy(&nv->value, &nvm.byte_array, NVM_VALUE_LEN); - return (STAT_OK); +stat_t read_persistent_value(nvObj_t *nv) { + nvm.address = nvm.profile_base + (nv->index * NVM_VALUE_LEN); + EEPROM_ReadBytes(nvm.address, nvm.byte_array, NVM_VALUE_LEN); + memcpy(&nv->value, &nvm.byte_array, NVM_VALUE_LEN); + return STAT_OK; } -stat_t write_persistent_value(nvObj_t *nv) -{ - if (cm.cycle_state != CYCLE_OFF) - return(rpt_exception(STAT_FILE_NOT_OPEN)); // can't write when machine is moving - nvm.tmp_value = nv->value; - ritorno(read_persistent_value(nv)); - if ((isnan((double)nv->value)) || (isinf((double)nv->value)) || (fp_NE(nv->value, nvm.tmp_value))) { - memcpy(&nvm.byte_array, &nvm.tmp_value, NVM_VALUE_LEN); - nvm.address = nvm.profile_base + (nv->index * NVM_VALUE_LEN); - (void)EEPROM_WriteBytes(nvm.address, nvm.byte_array, NVM_VALUE_LEN); - } - nv->value =nvm.tmp_value; // always restore value - return (STAT_OK); + +stat_t write_persistent_value(nvObj_t *nv) { + if (cm.cycle_state != CYCLE_OFF) + return(rpt_exception(STAT_FILE_NOT_OPEN)); // can't write when machine is moving + + nvm.tmp_value = nv->value; + ritorno(read_persistent_value(nv)); + if ((isnan((double)nv->value)) || (isinf((double)nv->value)) || (fp_NE(nv->value, nvm.tmp_value))) { + memcpy(&nvm.byte_array, &nvm.tmp_value, NVM_VALUE_LEN); + nvm.address = nvm.profile_base + (nv->index * NVM_VALUE_LEN); + EEPROM_WriteBytes(nvm.address, nvm.byte_array, NVM_VALUE_LEN); + } + nv->value =nvm.tmp_value; // always restore value + + return STAT_OK; } diff --git a/src/persistence.h b/src/persistence.h index db960b4..ae45688 100755 --- a/src/persistence.h +++ b/src/persistence.h @@ -28,24 +28,24 @@ #ifndef PERSISTENCE_H_ONCE #define PERSISTENCE_H_ONCE -#include "config.h" // needed for nvObj_t definition +#include "config.h" // needed for nvObj_t definition -#define NVM_VALUE_LEN 4 // NVM value length (float, fixed length) -#define NVM_BASE_ADDR 0x0000 // base address of usable NVM +#define NVM_VALUE_LEN 4 // NVM value length (float, fixed length) +#define NVM_BASE_ADDR 0x0000 // base address of usable NVM //**** persistence singleton **** typedef struct nvmSingleton { - uint16_t base_addr; // NVM base address - uint16_t profile_base; // NVM base address of current profile] - uint16_t address; - float tmp_value; - int8_t byte_array[NVM_VALUE_LEN]; + uint16_t base_addr; // NVM base address + uint16_t profile_base; // NVM base address of current profile] + uint16_t address; + float tmp_value; + int8_t byte_array[NVM_VALUE_LEN]; } nvmSingleton_t; //**** persistence function prototypes **** -void persistence_init(void); +void persistence_init(); stat_t read_persistent_value(nvObj_t *nv); stat_t write_persistent_value(nvObj_t *nv); diff --git a/src/plan_arc.c b/src/plan_arc.c index 0e7d54c..beda066 100755 --- a/src/plan_arc.c +++ b/src/plan_arc.c @@ -33,18 +33,18 @@ arc_t arc; // Local functions -static stat_t _compute_arc(void); -static stat_t _compute_arc_offsets_from_radius(void); -static void _estimate_arc_time(void); -//static stat_t _test_arc_soft_limits(void); +static stat_t _compute_arc(); +static stat_t _compute_arc_offsets_from_radius(); +static void _estimate_arc_time(); +//static stat_t _test_arc_soft_limits(); /***************************************************************************** * Canonical Machining arc functions (arc prep for planning and runtime) * - * cm_arc_init() - initialize arcs - * cm_arc_feed() - canonical machine entry point for arc + * cm_arc_init() - initialize arcs + * cm_arc_feed() - canonical machine entry point for arc * cm_arc_callback() - mail-loop callback for arc generation - * cm_abort_arc() - stop an arc in process + * cm_abort_arc() - stop an arc in process */ /* @@ -52,8 +52,8 @@ static void _estimate_arc_time(void); */ void cm_arc_init() { - arc.magic_start = MAGICNUM; - arc.magic_end = MAGICNUM; + arc.magic_start = MAGICNUM; + arc.magic_end = MAGICNUM; } /* @@ -63,163 +63,163 @@ void cm_arc_init() * 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 + 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 + //////////////////////////////////////////////////// + // 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))) { - return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED); - } + // trap missing 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 + 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 - return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE); + 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_y = fp_NOT_ZERO(flags[AXIS_Y]); - bool target_z = fp_NOT_ZERO(flags[AXIS_Z]); + bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // set true if X axis has been 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_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 - // 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 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Y; - arc.linear_axis = AXIS_Z; + // 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 + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Y; + arc.linear_axis = AXIS_Z; if (radius_f) { if (!(target_x || target_y)) { // must have at least one endpoint specified - return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE); + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; } } else { // center format arc tests if (offset_k) { // it's OK to be missing either or both i and j, but error if k is present - return (STAT_ARC_SPECIFICATION_ERROR); + return STAT_ARC_SPECIFICATION_ERROR; } } - } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_Y; + } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_Y; if (radius_f) { if (!(target_x || target_z)) - return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE); + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; } else { if (offset_j) - return (STAT_ARC_SPECIFICATION_ERROR); + return STAT_ARC_SPECIFICATION_ERROR; } - } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 - arc.plane_axis_0 = AXIS_Y; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_X; + } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 + arc.plane_axis_0 = AXIS_Y; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_X; if (radius_f) { if (!(target_y || target_z)) - return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE); + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; } else { if (offset_i) - return (STAT_ARC_SPECIFICATION_ERROR); + return STAT_ARC_SPECIFICATION_ERROR; } - } + } - // set values in the Gcode model state & copy it (linenum was already captured) - cm_set_model_target(target, flags); + // 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 if(radius_f) { if ((fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X])) && (fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y])) && (fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z]))) { - return (STAT_ARC_ENDPOINT_IS_STARTING_POINT); + return STAT_ARC_ENDPOINT_IS_STARTING_POINT; } } // 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.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 - arc.radius = _to_millimeters(radius); // set arc radius or zero + 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[1] = _to_millimeters(j); - arc.offset[2] = _to_millimeters(k); + arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to 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 a positive integer - force it if not - // 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])); + // 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])); - // compute arc runtime values - ritorno(_compute_arc()); + // compute arc runtime values + ritorno(_compute_arc()); - if (fp_ZERO(arc.length)) { - return (STAT_MINIMUM_LENGTH_MOVE); // trap zero length arcs that _compute_arc can throw + if (fp_ZERO(arc.length)) { + return STAT_MINIMUM_LENGTH_MOVE; // trap zero length arcs that _compute_arc can throw } - cm_cycle_start(); // if not already started - arc.run_state = MOVE_RUN; // enable arc to be run from the callback - cm_finalize_move(); - return (STAT_OK); + cm_cycle_start(); // if not already started + arc.run_state = MOVE_RUN; // enable arc to be run from the callback + cm_finalize_move(); + return STAT_OK; } /* * cm_arc_callback() - generate an arc * - * cm_arc_callback() is 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. + * cm_arc_callback() is 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. * * Parts of this routine were originally sourced from the grbl project. */ stat_t cm_arc_callback() { - if (arc.run_state == MOVE_OFF) - return (STAT_NOOP); - - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) - return (STAT_EAGAIN); - - arc.theta += arc.arc_segment_theta; - 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 - - if (--arc.arc_segment_count > 0) - return (STAT_EAGAIN); - arc.run_state = MOVE_OFF; - return (STAT_OK); + if (arc.run_state == MOVE_OFF) + return STAT_NOOP; + + if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) + return STAT_EAGAIN; + + arc.theta += arc.arc_segment_theta; + 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 + + if (--arc.arc_segment_count > 0) + return STAT_EAGAIN; + arc.run_state = MOVE_OFF; + return STAT_OK; } /* * cm_abort_arc() - stop arc movement without maintaining position * - * OK to call if no arc is running + * OK to call if no arc is running */ void cm_abort_arc() { - arc.run_state = MOVE_OFF; + arc.run_state = MOVE_OFF; } /* * _compute_arc() - 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 * * * * @@ -234,7 +234,7 @@ void cm_abort_arc() static stat_t _compute_arc() { - // Compute radius. A non-zero radius value indicates a radius arc + // Compute radius. A non-zero radius value indicates a radius arc if (fp_NOT_ZERO(arc.radius)) { // indicates a radius arc _compute_arc_offsets_from_radius(); } else { // compute start radius @@ -254,20 +254,20 @@ static stat_t _compute_arc() if ( (err > ARC_RADIUS_ERROR_MAX) || ((err < ARC_RADIUS_ERROR_MIN) && (err > arc.radius * ARC_RADIUS_TOLERANCE)) ) { -// return (STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT); - return (STAT_ARC_SPECIFICATION_ERROR); +// return STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT; + 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) + // 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]); // 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)) { // handle the valid case of a full circle arc w/P=0 + 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)) { // handle the valid case of a full circle arc w/P=0 arc.rotations = 1.0; } } else { // ... it's not a full circle @@ -275,17 +275,17 @@ static stat_t _compute_arc() // 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) + arc.angular_travel = 0; // very large radii arcs can have zero angular travel (thanks PartKam) } else { - if (arc.theta_end < arc.theta) { // make the difference positive so we have clockwise travel + if (arc.theta_end < arc.theta) { // make the difference positive so we have clockwise travel arc.theta_end += (2*M_PI * g18_correction); } - arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel - if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { // reverse travel direction if it's CCW arc + arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel + if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { // reverse travel direction if it's CCW arc arc.angular_travel -= (2*M_PI * g18_correction); } } - } + } // Add in travel for rotations if (cm.gm.motion_mode == MOTION_MODE_CW_ARC) { @@ -294,54 +294,54 @@ static stat_t _compute_arc() 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]; - arc.planar_travel = arc.angular_travel * arc.radius; - arc.length = hypotf(arc.planar_travel, arc.linear_travel); // NB: hypot is insensitive to +/- signs - _estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation - - // 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)); - float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len; - float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; - - arc.arc_segments = floor(min3(arc_segments_for_chordal_accuracy, - 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_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; + // 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); // NB: hypot is insensitive to +/- signs + _estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation + + // 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)); + float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len; + float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; + + arc.arc_segments = floor(min3(arc_segments_for_chordal_accuracy, + 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_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 - return (STAT_OK); + arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target + return STAT_OK; } /* * _compute_arc_offsets_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 + * 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: + * ` [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. + * 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 - * i == x/2 - y/d*h - * j == y/2 + x/d*h + * d^2 == x^2 + y^2 + * h^2 == r^2 - (d/2)^2 + * i == x/2 - y/d*h + * j == y/2 + x/d*h * O <- [i,j] * - | * r - | @@ -351,39 +351,39 @@ static stat_t _compute_arc() * [0,0] -> C -----------------+--------------- T <- [x,y] * | <------ d/2 ---->| * - * C - Current position - * T - Target position - * O - center of circle that pass through both C and T - * d - distance from C to T - * r - designated radius - * h - distance from center of CT to O + * C - Current position + * T - Target position + * O - center of circle that pass through both C and T + * d - distance from C to T + * r - designated radius + * h - distance from center of CT to O * - * 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 + * 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: - * 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: + * 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: - * 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 + * 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 ---- * - * 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 * * ^ * Clockwise circles with | Clockwise circles with - * this center will have | this center will have + * this center will have | this center will have * > 180 deg of angular travel | < 180 deg of angular travel, * \ | which is a good thing! * \ | / @@ -393,72 +393,72 @@ static stat_t _compute_arc() * C <-- Current position * * - * Assumes arc singleton has been pre-loaded with target and position. - * Parts of this routine were originally sourced from the grbl project. + * Assumes arc singleton has been pre-loaded with target and position. + * Parts of this routine were originally sourced from the grbl project. */ static stat_t _compute_arc_offsets_from_radius() { - // Calculate the change in position along each selected axis - float x = cm.gm.target[arc.plane_axis_0] - cm.gmx.position[arc.plane_axis_0]; - float y = cm.gm.target[arc.plane_axis_1] - cm.gmx.position[arc.plane_axis_1]; - - // *** From Forrest Green - Other Machine Co, 3/27/14 - // 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. - 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) - 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 - // of travel" (go figure!), even though it is advised against ever generating - // such circles in a single line of g-code. By inverting the sign of - // h_x2_div_d the center of the circles is placed on the opposite side of - // the line of travel and thus we get the unadvisably long arcs as prescribed. - if (arc.radius < 0) { h_x2_div_d = -h_x2_div_d; } - - // Complete the operation by calculating the actual center of the arc - arc.offset[arc.plane_axis_0] = (x-(y*h_x2_div_d))/2; - arc.offset[arc.plane_axis_1] = (y+(x*h_x2_div_d))/2; - arc.offset[arc.linear_axis] = 0; - return (STAT_OK); + // Calculate the change in position along each selected axis + float x = cm.gm.target[arc.plane_axis_0] - cm.gmx.position[arc.plane_axis_0]; + float y = cm.gm.target[arc.plane_axis_1] - cm.gmx.position[arc.plane_axis_1]; + + // *** From Forrest Green - Other Machine Co, 3/27/14 + // 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. + 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) + 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 + // of travel" (go figure!), even though it is advised against ever generating + // such circles in a single line of g-code. By inverting the sign of + // h_x2_div_d the center of the circles is placed on the opposite side of + // the line of travel and thus we get the unadvisably long arcs as prescribed. + if (arc.radius < 0) { h_x2_div_d = -h_x2_div_d; } + + // Complete the operation by calculating the actual center of the arc + arc.offset[arc.plane_axis_0] = (x-(y*h_x2_div_d))/2; + arc.offset[arc.plane_axis_1] = (y+(x*h_x2_div_d))/2; + arc.offset[arc.linear_axis] = 0; + return STAT_OK; } /* * _estimate_arc_time () * - * 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 - 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)); - } + // 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 + 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)); + } } diff --git a/src/plan_arc.h b/src/plan_arc.h index 43df06d..3376066 100755 --- a/src/plan_arc.h +++ b/src/plan_arc.h @@ -28,50 +28,50 @@ // See planner.h for MM_PER_ARC_SEGMENT and other arc setting #defines -typedef struct arArcSingleton { // persistent planner and runtime variables - magic_t magic_start; - uint8_t run_state; // runtime state machine sequence +typedef struct arArcSingleton { // persistent planner and runtime variables + magic_t magic_start; + uint8_t run_state; // runtime state machine sequence - float position[AXES]; // accumulating runtime position - float offset[3]; // IJK offsets + float position[AXES]; // accumulating runtime position + float offset[3]; // IJK offsets - float length; // length of line or helix in mm - float theta; // total angle specified by arc - float theta_end; - float radius; // Raw R value, or computed via offsets - float angular_travel; // travel along the arc - float linear_travel; // travel along linear axis of arc + float length; // length of line or helix in mm + float theta; // total angle specified by arc + float theta_end; + float radius; // Raw R value, or computed via offsets + float angular_travel; // travel along the arc + float linear_travel; // travel along linear axis of arc float planar_travel; - uint8_t full_circle; // set true if full circle arcs specified - uint32_t rotations; // Number of full rotations for full circles (P value) + uint8_t full_circle; // set true if full circle arcs specified + uint32_t rotations; // Number of full rotations for full circles (P value) - uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 - uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 - uint8_t linear_axis; // linear axis (normal to plane) + uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 + uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 + uint8_t linear_axis; // linear axis (normal to plane) - float arc_time; // total running time for arc (derived) - float arc_segments; // number of segments in arc or blend - int32_t arc_segment_count; // count of running segments - float arc_segment_theta; // angular motion per segment - float arc_segment_linear_travel;// linear motion per segment - float center_0; // center of circle at plane axis 0 (e.g. X for G17) - float center_1; // center of circle at plane axis 1 (e.g. Y for G17) + float arc_time; // total running time for arc (derived) + float arc_segments; // number of segments in arc or blend + int32_t arc_segment_count; // count of running segments + float arc_segment_theta; // angular motion per segment + float arc_segment_linear_travel;// linear motion per segment + float center_0; // center of circle at plane axis 0 (e.g. X for G17) + float center_1; // center of circle at plane axis 1 (e.g. Y for G17) - GCodeState_t gm; // Gcode state struct is passed for each arc segment. Usage: -// uint32_t linenum; // line number of the arc feed move - same for each segment -// float target[AXES]; // arc segment target -// float work_offset[AXES]; // offset from machine coord system for reporting (same for each segment) -// float move_time; // segment_time: constant time per aline segment + GCodeState_t gm; // Gcode state struct is passed for each arc segment. Usage: +// uint32_t linenum; // line number of the arc feed move - same for each segment +// float target[AXES]; // arc segment target +// float work_offset[AXES]; // offset from machine coord system for reporting (same for each segment) +// float move_time; // segment_time: constant time per aline segment - magic_t magic_end; + magic_t magic_end; } arc_t; extern arc_t arc; -/* arc function prototypes */ // NOTE: See canonical_machine.h for cm_arc_feed() prototype +/* arc function prototypes */ // NOTE: See canonical_machine.h for cm_arc_feed() prototype -void cm_arc_init(void); -stat_t cm_arc_callback(void); -void cm_abort_arc(void); +void cm_arc_init(); +stat_t cm_arc_callback(); +void cm_abort_arc(); -#endif // End of include guard: PLAN_ARC_H_ONCE +#endif // End of include guard: PLAN_ARC_H_ONCE diff --git a/src/plan_exec.c b/src/plan_exec.c index 7454d54..39f8ca6 100755 --- a/src/plan_exec.c +++ b/src/plan_exec.c @@ -36,10 +36,10 @@ #include "util.h" // execute routines (NB: These are all called from the LO interrupt) -static stat_t _exec_aline_head(void); -static stat_t _exec_aline_body(void); -static stat_t _exec_aline_tail(void); -static stat_t _exec_aline_segment(void); +static stat_t _exec_aline_head(); +static stat_t _exec_aline_body(); +static stat_t _exec_aline_tail(); +static stat_t _exec_aline_segment(); #ifndef __JERK_EXEC static void _init_forward_diffs(float Vi, float Vt); @@ -48,26 +48,26 @@ static void _init_forward_diffs(float Vi, float Vt); /************************************************************************* * mp_exec_move() - execute runtime functions to prep move for steppers * - * Dequeues the buffer queue and executes the move continuations. - * Manages run buffers and other details + * Dequeues the buffer queue and executes the move continuations. + * Manages run buffers and other details */ stat_t mp_exec_move() { - mpBuf_t *bf; - - if ((bf = mp_get_run_buffer()) == NULL) { // NULL means nothing's running - st_prep_null(); - return (STAT_NOOP); - } - // Manage cycle and motion state transitions - if (bf->move_type == MOVE_TYPE_ALINE) { // cycle auto-start for lines only - if (cm.motion_state == MOTION_STOP) cm_set_motion_state(MOTION_RUN); - } - if (bf->bf_func == NULL) + mpBuf_t *bf; + + if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running + st_prep_null(); + return STAT_NOOP; + } + // Manage cycle and motion state transitions + if (bf->move_type == MOVE_TYPE_ALINE) { // cycle auto-start for lines only + if (cm.motion_state == MOTION_STOP) cm_set_motion_state(MOTION_RUN); + } + if (bf->bf_func == 0) return(cm_hard_alarm(STAT_INTERNAL_ERROR)); // never supposed to get here - return (bf->bf_func(bf)); // run the move callback in the planner buffer + return bf->bf_func(bf); // run the move callback in the planner buffer } /*************************************************************************/ @@ -75,172 +75,172 @@ 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 - * STAT_NOOP cause no operation from the steppers - do not load the move - * STAT_xxxxx fatal error. Ends the move and frees the bf buffer - * - * This routine is called from the (LO) interrupt level. The interrupt - * sequencing relies on the behaviors of the routines being exactly correct. - * Each call to _exec_aline() must execute and prep *one and only one* - * segment. If the segment is the not the last segment in the bf buffer the - * _aline() must return STAT_EAGAIN. If it's the last segment it must return - * STAT_OK. If it encounters a fatal error that would terminate the move it - * should return a valid error code. Failure to obey this will introduce - * subtle and very difficult to diagnose bugs (trust me on this). - * - * Note 1 Returning STAT_OK ends the move and frees the bf buffer. - * Returning STAT_OK at this point does NOT advance position meaning any - * position error will be compensated by the next move. - * - * Note 2 Solves a potential race condition where the current move ends but the - * new move has not started because the previous move is still being run - * by the steppers. Planning can overwrite the new move. + * _exec_aline() - acceleration line main routine + * _exec_aline_head() - helper for acceleration section + * _exec_aline_body() - helper for cruise section + * _exec_aline_tail() - helper for deceleration section + * _exec_aline_segment() - helper for running a segment + * + * Returns: + * STAT_OK move is done + * STAT_EAGAIN move is not finished - has more segments to run + * STAT_NOOP cause no operation from the steppers - do not load the move + * STAT_xxxxx fatal error. Ends the move and frees the bf buffer + * + * This routine is called from the (LO) interrupt level. The interrupt + * sequencing relies on the behaviors of the routines being exactly correct. + * Each call to _exec_aline() must execute and prep *one and only one* + * segment. If the segment is the not the last segment in the bf buffer the + * _aline() must return STAT_EAGAIN. If it's the last segment it must return + * STAT_OK. If it encounters a fatal error that would terminate the move it + * should return a valid error code. Failure to obey this will introduce + * subtle and very difficult to diagnose bugs (trust me on this). + * + * Note 1 Returning STAT_OK ends the move and frees the bf buffer. + * Returning STAT_OK at this point does NOT advance position meaning any + * position error will be compensated by the next move. + * + * Note 2 Solves a potential race condition where the current move ends but the + * new move has not started because the previous move is still being run + * by the steppers. Planning can overwrite the new move. */ /* OPERATION: - * Aline generates jerk-controlled S-curves as per Ed Red's course notes: - * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf - * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations - * - * A full trapezoid is divided into 5 periods Periods 1 and 2 are the - * first and second halves of the acceleration ramp (the concave and convex - * parts of the S curve in the "head"). Periods 3 and 4 are the first - * and second parts of the deceleration ramp (the tail). There is also - * a period for the constant-velocity plateau of the trapezoid (the body). - * There are various degraded trapezoids possible, including 2 section - * combinations (head and tail; head and body; body and tail), and single - * sections - any one of the three. - * - * 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 - * - * 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 the - * move, accel_time is the time valaue needed to compute the velocity - which - * takes the initial velocity into account (move_time does not need to). + * Aline generates jerk-controlled S-curves as per Ed Red's course notes: + * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf + * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations + * + * A full trapezoid is divided into 5 periods Periods 1 and 2 are the + * first and second halves of the acceleration ramp (the concave and convex + * parts of the S curve in the "head"). Periods 3 and 4 are the first + * and second parts of the deceleration ramp (the tail). There is also + * a period for the constant-velocity plateau of the trapezoid (the body). + * There are various degraded trapezoids possible, including 2 section + * combinations (head and tail; head and body; body and tail), and single + * sections - any one of the three. + * + * 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 + * + * 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 the + * move, accel_time is the time valaue needed to compute the velocity - which + * takes the initial velocity into account (move_time does not need to). */ /* --- State transitions - hierarchical state machine --- * - * bf->move_state transitions: - * from _NEW to _RUN on first call (sub_state set to _OFF) - * from _RUN to _OFF on final call - * or just remains _OFF + * bf->move_state transitions: + * from _NEW to _RUN on first call (sub_state set to _OFF) + * from _RUN to _OFF on final call + * 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 + * 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 + * Note: For a direct math implementation see build 357.xx or earlier + * Builds 358 onward have only forward difference code */ stat_t mp_exec_aline(mpBuf_t *bf) { - if (bf->move_state == MOVE_OFF) - return (STAT_NOOP); - - // start a new move by setting up local context (singleton) - if (mr.move_state == MOVE_OFF) { - if (cm.hold_state == FEEDHOLD_HOLD) - return (STAT_NOOP); // stops here if holding - - // initialization to process the new incoming bf buffer (Gcode block) - memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t));// copy in the gcode model state - bf->replannable = false; - // too short lines have already been removed - if (fp_ZERO(bf->length)) { // ...looks for an actual zero here - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; - bf->nx->replannable = false; // prevent overplanning (Note 2) - st_prep_null(); // call this to keep the loader happy - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty - return (STAT_NOOP); - } - bf->move_state = MOVE_RUN; - mr.move_state = MOVE_RUN; - mr.section = SECTION_HEAD; - mr.section_state = SECTION_NEW; - mr.jerk = bf->jerk; + if (bf->move_state == MOVE_OFF) + return STAT_NOOP; + + // start a new move by setting up local context (singleton) + if (mr.move_state == MOVE_OFF) { + if (cm.hold_state == FEEDHOLD_HOLD) + return STAT_NOOP; // stops here if holding + + // initialization to process the new incoming bf buffer (Gcode block) + memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t));// copy in the gcode model state + bf->replannable = false; + // too short lines have already been removed + if (fp_ZERO(bf->length)) { // ...looks for an actual zero here + mr.move_state = MOVE_OFF; // reset mr buffer + mr.section_state = SECTION_OFF; + bf->nx->replannable = false; // prevent overplanning (Note 2) + st_prep_null(); // call this to keep the loader happy + if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty + return STAT_NOOP; + } + bf->move_state = MOVE_RUN; + mr.move_state = MOVE_RUN; + mr.section = SECTION_HEAD; + mr.section_state = SECTION_NEW; + mr.jerk = bf->jerk; #ifdef __JERK_EXEC - mr.jerk_div2 = bf->jerk/2; // only needed by __JERK_EXEC + mr.jerk_div2 = bf->jerk/2; // only needed by __JERK_EXEC #endif - mr.head_length = bf->head_length; - mr.body_length = bf->body_length; - mr.tail_length = bf->tail_length; - - mr.entry_velocity = bf->entry_velocity; - mr.cruise_velocity = bf->cruise_velocity; - mr.exit_velocity = bf->exit_velocity; - - copy_vector(mr.unit, bf->unit); - copy_vector(mr.target, bf->gm.target); // save the final target of the move - - // generate the waypoints for position correction at section ends - for (uint8_t axis=0; axismove_state Description - // ----------- -------------- ---------------------------------------- - // STAT_EAGAIN mr buffer has more segments to run - // STAT_OK MOVE_RUN mr and bf buffers are done - // STAT_OK MOVE_NEW mr done; bf must be run again (it's been reused) - - if (status == STAT_EAGAIN) { - sr_request_status_report(SR_TIMED_REQUEST); // continue reporting mr buffer - } else { - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; - bf->nx->replannable = false; // prevent overplanning (Note 2) - if (bf->move_state == MOVE_RUN) { - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty - } - } - return (status); + mr.head_length = bf->head_length; + mr.body_length = bf->body_length; + mr.tail_length = bf->tail_length; + + mr.entry_velocity = bf->entry_velocity; + mr.cruise_velocity = bf->cruise_velocity; + mr.exit_velocity = bf->exit_velocity; + + copy_vector(mr.unit, bf->unit); + copy_vector(mr.target, bf->gm.target); // save the final target of the move + + // generate the waypoints for position correction at section ends + for (uint8_t axis=0; axismove_state Description + // ----------- -------------- ---------------------------------------- + // STAT_EAGAIN mr buffer has more segments to run + // STAT_OK MOVE_RUN mr and bf buffers are done + // STAT_OK MOVE_NEW mr done; bf must be run again (it's been reused) + + if (status == STAT_EAGAIN) { + sr_request_status_report(SR_TIMED_REQUEST); // continue reporting mr buffer + } else { + mr.move_state = MOVE_OFF; // reset mr buffer + mr.section_state = SECTION_OFF; + bf->nx->replannable = false; // prevent overplanning (Note 2) + if (bf->move_state == MOVE_RUN) { + if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty + } + } + return status; } /* Forward difference math explained: * - * We are using a quintic (fifth-degree) Bezier polynomial for the velocity curve. - * This gives us a "linear pop" velocity curve; with pop being the sixth derivative of position: - * velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th + * We are using a quintic (fifth-degree) Bezier polynomial for the velocity curve. + * This gives us a "linear pop" velocity curve; with pop being the sixth derivative of position: + * velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th * * The Bezier curve takes the form: * @@ -249,91 +249,91 @@ stat_t mp_exec_aline(mpBuf_t *bf) * 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 + * 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: + * 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 - * - * 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 - * D = 0 - * E = 0 - * F = P_i - * - * Given an interval count of I to get from P_i to P_t, we get the parametric "step" size of h = 1/I. - * We need to calculate the initial value of forward differences (F_0 - F_5) such that the inital - * velocity V = P_i, then we iterate over the following I times: - * - * V += F_5 - * F_5 += F_4 - * F_4 += F_3 - * F_3 += F_2 - * F_2 += F_1 - * - * See http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 for an example of - * how to calculate F_0 - F_5 for a cubic bezier curve. Since this is a quintic bezier curve, we - * need to extend the formulas somewhat. I'll not go into the long-winded 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 + (10ah^3 + 6bh^2 + 3ch)t^2 + - * (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + bh^4 + ch^3 + dh^2 + eh - * - * a = 5ah - * b = 10ah^2 + 4bh - * c = 10ah^3 + 6bh^2 + 3ch - * d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh + * 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 + * D = 0 + * E = 0 + * F = P_i + * + * Given an interval count of I to get from P_i to P_t, we get the parametric "step" size of h = 1/I. + * We need to calculate the initial value of forward differences (F_0 - F_5) such that the inital + * velocity V = P_i, then we iterate over the following I times: + * + * V += F_5 + * F_5 += F_4 + * F_4 += F_3 + * F_3 += F_2 + * F_2 += F_1 + * + * See http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 for an example of + * how to calculate F_0 - F_5 for a cubic bezier curve. Since this is a quintic bezier curve, we + * need to extend the formulas somewhat. I'll not go into the long-winded 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 + (10ah^3 + 6bh^2 + 3ch)t^2 + + * (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + bh^4 + ch^3 + dh^2 + eh + * + * a = 5ah + * b = 10ah^2 + 4bh + * c = 10ah^3 + 6bh^2 + 3ch + * 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 + (70ah^4 + 24bh^3 + 6ch^2)t + - * 30ah^5 + 14bh^4 + 6ch^3 + 2dh^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 + * 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 "averaging" the velocity of each segment, we actually want to have 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_3 = 255Ah^5 + 48Bh^4 + 6Ch^3 - * F_2 = 300Ah^5 + 24Bh^4 - * F_1 = 120Ah^5 + * 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. */ @@ -341,39 +341,39 @@ stat_t mp_exec_aline(mpBuf_t *bf) static void _init_forward_diffs(float Vi, float Vt) { - float A = -6.0*Vi + 6.0*Vt; - float B = 15.0*Vi - 15.0*Vt; - float C = -10.0*Vi + 10.0*Vt; - // D = 0 - // E = 0 - // F = Vi + float A = -6.0*Vi + 6.0*Vt; + float B = 15.0*Vi - 15.0*Vt; + float C = -10.0*Vi + 10.0*Vt; + // D = 0 + // E = 0 + // F = Vi - float h = 1/(mr.segments); + float h = 1/(mr.segments); - float Ah_5 = A * h * h * h * h * h; - float Bh_4 = B * h * h * h * h; - float Ch_3 = C * h * h * h; + float Ah_5 = A * h * h * h * h * h; + float Bh_4 = B * h * h * h * h; + float Ch_3 = C * h * h * h; - mr.forward_diff_5 = (121.0/16.0)*Ah_5 + 5.0*Bh_4 + (13.0/4.0)*Ch_3; - mr.forward_diff_4 = (165.0/2.0)*Ah_5 + 29.0*Bh_4 + 9.0*Ch_3; - mr.forward_diff_3 = 255.0*Ah_5 + 48.0*Bh_4 + 6.0*Ch_3; - mr.forward_diff_2 = 300.0*Ah_5 + 24.0*Bh_4; - mr.forward_diff_1 = 120.0*Ah_5; + mr.forward_diff_5 = (121.0/16.0)*Ah_5 + 5.0*Bh_4 + (13.0/4.0)*Ch_3; + mr.forward_diff_4 = (165.0/2.0)*Ah_5 + 29.0*Bh_4 + 9.0*Ch_3; + mr.forward_diff_3 = 255.0*Ah_5 + 48.0*Bh_4 + 6.0*Ch_3; + mr.forward_diff_2 = 300.0*Ah_5 + 24.0*Bh_4; + mr.forward_diff_1 = 120.0*Ah_5; #ifdef __KAHAN - mr.forward_diff_5_c = 0; - mr.forward_diff_4_c = 0; - mr.forward_diff_3_c = 0; - mr.forward_diff_2_c = 0; - mr.forward_diff_1_c = 0; + mr.forward_diff_5_c = 0; + mr.forward_diff_4_c = 0; + mr.forward_diff_3_c = 0; + mr.forward_diff_2_c = 0; + mr.forward_diff_1_c = 0; #endif - // Calculate the initial velocity by calculating V(h/2) - float half_h = h/2.0; - float half_Ch_3 = C * half_h * half_h * half_h; - float half_Bh_4 = B * half_h * half_h * half_h * half_h; - 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; + // Calculate the initial velocity by calculating V(h/2) + float half_h = h/2.0; + float half_Ch_3 = C * half_h * half_h * half_h; + float half_Bh_4 = B * half_h * half_h * half_h * half_h; + 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 @@ -384,161 +384,161 @@ static void _init_forward_diffs(float Vi, float Vt) 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; - mr.gm.move_time = mr.head_length / mr.midpoint_velocity; // time for entire accel region - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); // # of segments in *each half* - 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; - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; // elapsed time starting point (offset) - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) + 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; + mr.gm.move_time = mr.head_length / mr.midpoint_velocity; // time for entire accel region + mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); // # of segments in *each half* + 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; + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; // elapsed time starting point (offset) + 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; - } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) - 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; - mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment - } - return(STAT_EAGAIN); - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HAF (convex part of accel curve) - 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))) + mr.section = SECTION_HEAD; + mr.section_state = SECTION_1st_HALF; + } + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) + 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; + mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment + } + return(STAT_EAGAIN); + } + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HAF (convex part of accel curve) + 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); + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + } + } + return(STAT_EAGAIN); } #else // __ JERK_EXEC static stat_t _exec_aline_head() { - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) - if (fp_ZERO(mr.head_length)) { - mr.section = SECTION_BODY; - return(_exec_aline_body()); // skip ahead to the body generator - } - mr.gm.move_time = 2*mr.head_length / (mr.entry_velocity + mr.cruise_velocity);// time for entire accel region - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section - mr.segment_time = mr.gm.move_time / mr.segments; - _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) + 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.gm.move_time = 2*mr.head_length / (mr.entry_velocity + mr.cruise_velocity);// time for entire accel region + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section + mr.segment_time = mr.gm.move_time / mr.segments; + _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); + mr.segment_count = (uint32_t)mr.segments; + 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; // Note: Set to SECTION_1st_HALF for one segment - } - // 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. - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) - if (_exec_aline_segment() == STAT_OK) { // set up for second half - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - } else { - mr.section_state = SECTION_2nd_HALF; - } - return(STAT_EAGAIN); - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF (convex part of accel curve) + mr.section = SECTION_HEAD; + mr.section_state = SECTION_1st_HALF; // Note: Set to SECTION_1st_HALF for one segment + } + // 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. + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) + if (_exec_aline_segment() == STAT_OK) { // set up for second half + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + } else { + mr.section_state = SECTION_2nd_HALF; + } + return(STAT_EAGAIN); + } + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF (convex part of accel curve) #ifndef __KAHAN - mr.segment_velocity += mr.forward_diff_5; -#else // Use Kahan summation algorithm to mitigate floating-point errors for the above - float y = mr.forward_diff_5 - mr.forward_diff_5_c; - float v = mr.segment_velocity + y; - mr.forward_diff_5_c = (v - mr.segment_velocity) - y; - mr.segment_velocity = v; + mr.segment_velocity += mr.forward_diff_5; +#else // Use Kahan summation algorithm to mitigate floating-point errors for the above + float y = mr.forward_diff_5 - mr.forward_diff_5_c; + float v = mr.segment_velocity + y; + mr.forward_diff_5_c = (v - mr.segment_velocity) - y; + mr.segment_velocity = v; #endif - if (_exec_aline_segment() == STAT_OK) { // set up for body - if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length))) + if (_exec_aline_segment() == STAT_OK) { // set up for body + 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; - } else { + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + } else { #ifndef __KAHAN - mr.forward_diff_5 += mr.forward_diff_4; - mr.forward_diff_4 += mr.forward_diff_3; - mr.forward_diff_3 += mr.forward_diff_2; - mr.forward_diff_2 += mr.forward_diff_1; + mr.forward_diff_5 += mr.forward_diff_4; + mr.forward_diff_4 += mr.forward_diff_3; + mr.forward_diff_3 += mr.forward_diff_2; + mr.forward_diff_2 += mr.forward_diff_1; #else - //mr.forward_diff_5 += mr.forward_diff_4; - y = mr.forward_diff_4 - mr.forward_diff_4_c; - v = mr.forward_diff_5 + y; - mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; - mr.forward_diff_5 = v; - - //mr.forward_diff_4 += mr.forward_diff_3; - y = mr.forward_diff_3 - mr.forward_diff_3_c; - v = mr.forward_diff_4 + y; - mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; - mr.forward_diff_4 = v; - - //mr.forward_diff_3 += mr.forward_diff_2; - y = mr.forward_diff_2 - mr.forward_diff_2_c; - v = mr.forward_diff_3 + y; - mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; - mr.forward_diff_3 = v; - - //mr.forward_diff_2 += mr.forward_diff_1; - y = mr.forward_diff_1 - mr.forward_diff_1_c; - v = mr.forward_diff_2 + y; - mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; - mr.forward_diff_2 = v; + //mr.forward_diff_5 += mr.forward_diff_4; + y = mr.forward_diff_4 - mr.forward_diff_4_c; + v = mr.forward_diff_5 + y; + mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; + mr.forward_diff_5 = v; + + //mr.forward_diff_4 += mr.forward_diff_3; + y = mr.forward_diff_3 - mr.forward_diff_3_c; + v = mr.forward_diff_4 + y; + mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; + mr.forward_diff_4 = v; + + //mr.forward_diff_3 += mr.forward_diff_2; + y = mr.forward_diff_2 - mr.forward_diff_2_c; + v = mr.forward_diff_3 + y; + mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; + mr.forward_diff_3 = v; + + //mr.forward_diff_2 += mr.forward_diff_1; + y = mr.forward_diff_1 - mr.forward_diff_1_c; + v = mr.forward_diff_2 + y; + mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; + mr.forward_diff_2 = v; #endif - } - } - return(STAT_EAGAIN); + } + } + return(STAT_EAGAIN); } #endif // __ JERK_EXEC /********************************************************************************************* * _exec_aline_body() * - * 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 + * 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) + 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; - mr.section_state = SECTION_2nd_HALF; // uses PERIOD_2 so last segment detection works - } - 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); + mr.section = SECTION_BODY; + mr.section_state = SECTION_2nd_HALF; // uses PERIOD_2 so last segment detection works + } + 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); } /********************************************************************************************* @@ -549,116 +549,116 @@ static stat_t _exec_aline_body() 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; - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));// # of segments in *each half* - mr.segment_time = mr.gm.move_time / (2 * mr.segments); // time to advance for each segment - 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; - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; //compute time from midpoint of segment - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) + 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; + mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));// # of segments in *each half* + mr.segment_time = mr.gm.move_time / (2 * mr.segments); // time to advance for each segment + 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; + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; //compute time from midpoint of segment + 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_TAIL; - mr.section_state = SECTION_1st_HALF; - } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) - 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; - mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment - } - return(STAT_EAGAIN); - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) - 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 + mr.section = SECTION_TAIL; + mr.section_state = SECTION_1st_HALF; + } + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) + 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; + mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment + } + return(STAT_EAGAIN); + } + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) + 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 static stat_t _exec_aline_tail() { - if (mr.section_state == SECTION_NEW) { // INITIALIZATION - if (fp_ZERO(mr.tail_length)) + if (mr.section_state == SECTION_NEW) { // INITIALIZATION + if (fp_ZERO(mr.tail_length)) return(STAT_OK); // end the move - mr.gm.move_time = 2*mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); // len/avg. velocity - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section - mr.segment_time = mr.gm.move_time / mr.segments; // time to advance for each segment - _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) + mr.gm.move_time = 2*mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); // len/avg. velocity + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section + mr.segment_time = mr.gm.move_time / mr.segments; // time to advance for each segment + _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 without advancing position - mr.section = SECTION_TAIL; - mr.section_state = SECTION_1st_HALF; - } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) - if (_exec_aline_segment() == STAT_OK) { - // 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. - // Show that we did complete section 2 ... effectively. - mr.section_state = SECTION_2nd_HALF; - return STAT_OK; - } else { - mr.section_state = SECTION_2nd_HALF; - } - return(STAT_EAGAIN); - } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) + mr.section = SECTION_TAIL; + mr.section_state = SECTION_1st_HALF; + } + if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) + if (_exec_aline_segment() == STAT_OK) { + // 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. + // Show that we did complete section 2 ... effectively. + mr.section_state = SECTION_2nd_HALF; + return STAT_OK; + } else { + mr.section_state = SECTION_2nd_HALF; + } + return(STAT_EAGAIN); + } + if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) #ifndef __KAHAN - mr.segment_velocity += mr.forward_diff_5; -#else // Use Kahan summation algorithm to mitigate floating-point errors for the above - float y = mr.forward_diff_5 - mr.forward_diff_5_c; - float v = mr.segment_velocity + y; - mr.forward_diff_5_c = (v - mr.segment_velocity) - y; - mr.segment_velocity = v; + mr.segment_velocity += mr.forward_diff_5; +#else // Use Kahan summation algorithm to mitigate floating-point errors for the above + float y = mr.forward_diff_5 - mr.forward_diff_5_c; + float v = mr.segment_velocity + y; + mr.forward_diff_5_c = (v - mr.segment_velocity) - y; + mr.segment_velocity = v; #endif - if (_exec_aline_segment() == STAT_OK) { // set up for body - return STAT_OK; - } else { + if (_exec_aline_segment() == STAT_OK) { // set up for body + return STAT_OK; + } else { #ifndef __KAHAN - mr.forward_diff_5 += mr.forward_diff_4; - mr.forward_diff_4 += mr.forward_diff_3; - mr.forward_diff_3 += mr.forward_diff_2; - mr.forward_diff_2 += mr.forward_diff_1; + mr.forward_diff_5 += mr.forward_diff_4; + mr.forward_diff_4 += mr.forward_diff_3; + mr.forward_diff_3 += mr.forward_diff_2; + mr.forward_diff_2 += mr.forward_diff_1; #else - //mr.forward_diff_5 += mr.forward_diff_4; - y = mr.forward_diff_4 - mr.forward_diff_4_c; - v = mr.forward_diff_5 + y; - mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; - mr.forward_diff_5 = v; - - //mr.forward_diff_4 += mr.forward_diff_3; - y = mr.forward_diff_3 - mr.forward_diff_3_c; - v = mr.forward_diff_4 + y; - mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; - mr.forward_diff_4 = v; - - //mr.forward_diff_3 += mr.forward_diff_2; - y = mr.forward_diff_2 - mr.forward_diff_2_c; - v = mr.forward_diff_3 + y; - mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; - mr.forward_diff_3 = v; - - //mr.forward_diff_2 += mr.forward_diff_1; - y = mr.forward_diff_1 - mr.forward_diff_1_c; - v = mr.forward_diff_2 + y; - mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; - mr.forward_diff_2 = v; + //mr.forward_diff_5 += mr.forward_diff_4; + y = mr.forward_diff_4 - mr.forward_diff_4_c; + v = mr.forward_diff_5 + y; + mr.forward_diff_4_c = (v - mr.forward_diff_5) - y; + mr.forward_diff_5 = v; + + //mr.forward_diff_4 += mr.forward_diff_3; + y = mr.forward_diff_3 - mr.forward_diff_3_c; + v = mr.forward_diff_4 + y; + mr.forward_diff_3_c = (v - mr.forward_diff_4) - y; + mr.forward_diff_4 = v; + + //mr.forward_diff_3 += mr.forward_diff_2; + y = mr.forward_diff_2 - mr.forward_diff_2_c; + v = mr.forward_diff_3 + y; + mr.forward_diff_2_c = (v - mr.forward_diff_3) - y; + mr.forward_diff_3 = v; + + //mr.forward_diff_2 += mr.forward_diff_1; + y = mr.forward_diff_1 - mr.forward_diff_1_c; + v = mr.forward_diff_2 + y; + mr.forward_diff_1_c = (v - mr.forward_diff_2) - y; + mr.forward_diff_2 = v; #endif - } - } - return(STAT_EAGAIN); // should never get here + } + } + return(STAT_EAGAIN); // should never get here } #endif // __JERK_EXEC @@ -667,65 +667,65 @@ static stat_t _exec_aline_tail() * * NOTES ON STEP ERROR CORRECTION: * - * The commanded_steps are the target_steps delayed by one more segment. - * This lines them up in time with the encoder readings so a following error can be generated + * The commanded_steps are the target_steps delayed by one more segment. + * This lines them up in time with the encoder readings so a following error can be generated * - * The following_error term is positive if the encoder reading is greater than (ahead of) - * the commanded steps, and negative (behind) if the encoder reading is less than the - * commanded steps. The following error is not affected by the direction of movement - - * it's purely a statement of relative position. Examples: + * The following_error term is positive if the encoder reading is greater than (ahead of) + * the commanded steps, and negative (behind) if the encoder reading is less than the + * commanded steps. The following error is not affected by the direction of movement - + * it's purely a statement of relative position. Examples: * * Encoder Commanded Following Err - * 100 90 +10 encoder is 10 steps ahead of commanded steps - * -90 -100 +10 encoder is 10 steps ahead of commanded steps - * 90 100 -10 encoder is 10 steps behind commanded steps - * -100 -90 -10 encoder is 10 steps behind commanded steps + * 100 90 +10 encoder is 10 steps ahead of commanded steps + * -90 -100 +10 encoder is 10 steps ahead of commanded steps + * 90 100 -10 encoder is 10 steps behind commanded steps + * -100 -90 -10 encoder is 10 steps behind commanded steps */ static stat_t _exec_aline_segment() { - uint8_t i; - float travel_steps[MOTORS]; - - // Set target position for the segment - // If the segment ends on a section waypoint synchronize to the head, body or tail end - // Otherwise if not at a section waypoint compute target from segment time and velocity - // Don't do waypoint correction if you are going into a hold. - - if ((--mr.segment_count == 0) && (mr.section_state == SECTION_2nd_HALF) && - (cm.motion_state == MOTION_RUN) && (cm.cycle_state == CYCLE_MACHINING)) { - copy_vector(mr.gm.target, mr.waypoint[mr.section]); - } else { - float segment_length = mr.segment_velocity * mr.segment_time; - for (i=0; ibody_length @@ -94,161 +94,161 @@ uint8_t mp_get_runtime_busy() */ stat_t mp_aline(GCodeState_t *gm_in) { - mpBuf_t *bf; // current move pointer - float exact_stop = 0; // preset this value OFF - float junction_velocity; - uint8_t mr_flag = false; - - // compute some reusable terms - float axis_length[AXES]; - float axis_square[AXES]; - float length_square = 0; - - for (uint8_t axis=0; axistarget[axis] - mm.position[axis]; - axis_square[axis] = square(axis_length[axis]); - length_square += axis_square[axis]; - } - float length = sqrt(length_square); - - if (fp_ZERO(length)) { -// sr_request_status_report(); - return (STAT_OK); - } - - // If _calc_move_times() says the move will take less than the minimum move time - // get a more accurate time estimate based on starting velocity and acceleration. - // The time of the move is determined by its initial velocity (Vi) and how much - // acceleration will be occur. For this we need to look at the exit velocity of - // the previous block. There are 3 possible cases: - // (1) There is no previous block. Vi = 0 - // (2) Previous block is optimally planned. Vi = previous block's exit_velocity - // (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity - - _calc_move_times(gm_in, axis_length, axis_square); // set move time and minimum time in the state - if (gm_in->move_time < MIN_BLOCK_TIME) { - float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move - float entry_velocity = 0; // pre-set as if no previous block - if ((bf = mp_get_run_buffer()) != NULL) { - if (bf->replannable == true) { // not optimally planned - entry_velocity = bf->entry_velocity + bf->delta_vmax; - } else { // optimally planned - entry_velocity = bf->exit_velocity; - } - } - float move_time = (2 * length) / (2*entry_velocity + delta_velocity);// compute execution time for this move - if (move_time < MIN_BLOCK_TIME) - return (STAT_MINIMUM_TIME_MOVE); - } - - // get a cleared buffer and setup move variables - if ((bf = mp_get_write_buffer()) == NULL) + mpBuf_t *bf; // current move pointer + float exact_stop = 0; // preset this value OFF + float junction_velocity; + uint8_t mr_flag = false; + + // compute some reusable terms + float axis_length[AXES]; + float axis_square[AXES]; + float length_square = 0; + + for (uint8_t axis=0; axistarget[axis] - mm.position[axis]; + axis_square[axis] = square(axis_length[axis]); + length_square += axis_square[axis]; + } + float length = sqrt(length_square); + + if (fp_ZERO(length)) { +// sr_request_status_report(); + return STAT_OK; + } + + // If _calc_move_times() says the move will take less than the minimum move time + // get a more accurate time estimate based on starting velocity and acceleration. + // The time of the move is determined by its initial velocity (Vi) and how much + // acceleration will be occur. For this we need to look at the exit velocity of + // the previous block. There are 3 possible cases: + // (1) There is no previous block. Vi = 0 + // (2) Previous block is optimally planned. Vi = previous block's exit_velocity + // (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity + + _calc_move_times(gm_in, axis_length, axis_square); // set move time and minimum time in the state + if (gm_in->move_time < MIN_BLOCK_TIME) { + float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move + float entry_velocity = 0; // pre-set as if no previous block + if ((bf = mp_get_run_buffer()) != 0) { + if (bf->replannable == true) { // not optimally planned + entry_velocity = bf->entry_velocity + bf->delta_vmax; + } else { // optimally planned + entry_velocity = bf->exit_velocity; + } + } + float move_time = (2 * length) / (2*entry_velocity + delta_velocity);// compute execution time for this move + if (move_time < MIN_BLOCK_TIME) + return STAT_MINIMUM_TIME_MOVE; + } + + // get a cleared buffer and setup move variables + if ((bf = mp_get_write_buffer()) == 0) return(cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // never supposed to fail - bf->bf_func = mp_exec_aline; // register the callback to the exec function - bf->length = length; - memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer - - // Compute the unit vector and find the right jerk to use (combined operations) - // To determine the jerk value to use for the block we want to find the axis for which - // the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which - // the time to decelerate from the target velocity to zero would be the longest. - // - // We can determine the "longest" deceleration in terms of time or distance. - // - // The math for time-to-decelerate T from speed S to speed E with constant - // jerk J is: - // - // T = 2*sqrt((S-E)/J[n]) - // - // Since E is always zero in this calculation, we can simplify: - // T = 2*sqrt(S/J[n]) - // - // The math for distance-to-decelerate l from speed S to speed E with constant - // jerk J is: - // - // l = (S+E)*sqrt((S-E)/J) - // - // Since E is always zero in this calculation, we can simplify: - // l = S*sqrt(S/J) - // - // The final value we want is to know which one is *longest*, compared to the others, - // so we don't need the actual value. This means that the scale of the value doesn't - // matter, so for T we can remove the "2 *" and For L we can remove the "S*". - // This leaves both to be simply "sqrt(S/J)". Since we don't need the scale, - // it doesn't matter what speed we're coming from, so S can be replaced with 1. - // - // However, we *do* need to compensate for the fact that each axis contributes - // differently to the move, so we will scale each contribution C[n] by the - // proportion of the axis movement length D[n] to the total length of the move L. - // Using that, we construct the following, with these definitions: - // - // J[n] = max_jerk for axis n - // D[n] = distance traveled for this move for axis n - // L = total length of the move in all axes - // C[n] = "axis contribution" of axis n - // - // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) - // - // Keeping in mind that we only need a rank compared to the other axes, we can further - // optimize the calculations:: - // - // Square the expression to remove the square root: - // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.) - // - // Re-arranged to optimize divides for pre-calculated values, we create a value - // M that we compute once: - // M = (1/(L^2)) - // Then we use it in the calculations for every axis: - // C[n] = (1/J[n]) * D[n]^2 * M - // - // Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further. - // - // Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value - // of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into - // its constituent axes for execution the jerk for that axis will be at it's maximum value. - - float C; // contribution term. C = T * a - float maxC = 0; - float recip_L2 = 1/length_square; - - for (uint8_t axis=0; axis 0) { // You cannot use the fp_XXX comparisons here! - bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero) - C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's positive - if (C > maxC) { - maxC = C; - bf->jerk_axis = axis; // also needed for junction vmax calculation - } - } - } - // set up and pre-compute the jerk terms needed for this round of planning - bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale the jerk - - if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta - mm.jerk = bf->jerk; // used before this point next time around - mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning - mm.cbrt_jerk = cbrt(bf->jerk); - } - bf->recip_jerk = mm.recip_jerk; - bf->cbrt_jerk = mm.cbrt_jerk; - - // finish up the current block variables - if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed - bf->replannable = true; - exact_stop = 8675309; // an arbitrarily large floating point number - } - bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested - junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); - bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); - bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); - bf->braking_velocity = bf->delta_vmax; - - // Note: these next lines must remain in exact order. Position must update before committing the buffer. - _plan_block_list(bf, &mr_flag); // replan block list - copy_vector(mm.position, bf->gm.target); // set the planner position - mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update) - return (STAT_OK); + bf->bf_func = mp_exec_aline; // register the callback to the exec function + bf->length = length; + memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer + + // Compute the unit vector and find the right jerk to use (combined operations) + // To determine the jerk value to use for the block we want to find the axis for which + // the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which + // the time to decelerate from the target velocity to zero would be the longest. + // + // We can determine the "longest" deceleration in terms of time or distance. + // + // The math for time-to-decelerate T from speed S to speed E with constant + // jerk J is: + // + // T = 2*sqrt((S-E)/J[n]) + // + // Since E is always zero in this calculation, we can simplify: + // T = 2*sqrt(S/J[n]) + // + // The math for distance-to-decelerate l from speed S to speed E with constant + // jerk J is: + // + // l = (S+E)*sqrt((S-E)/J) + // + // Since E is always zero in this calculation, we can simplify: + // l = S*sqrt(S/J) + // + // The final value we want is to know which one is *longest*, compared to the others, + // so we don't need the actual value. This means that the scale of the value doesn't + // matter, so for T we can remove the "2 *" and For L we can remove the "S*". + // This leaves both to be simply "sqrt(S/J)". Since we don't need the scale, + // it doesn't matter what speed we're coming from, so S can be replaced with 1. + // + // However, we *do* need to compensate for the fact that each axis contributes + // differently to the move, so we will scale each contribution C[n] by the + // proportion of the axis movement length D[n] to the total length of the move L. + // Using that, we construct the following, with these definitions: + // + // J[n] = max_jerk for axis n + // D[n] = distance traveled for this move for axis n + // L = total length of the move in all axes + // C[n] = "axis contribution" of axis n + // + // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) + // + // Keeping in mind that we only need a rank compared to the other axes, we can further + // optimize the calculations:: + // + // Square the expression to remove the square root: + // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.) + // + // Re-arranged to optimize divides for pre-calculated values, we create a value + // M that we compute once: + // M = (1/(L^2)) + // Then we use it in the calculations for every axis: + // C[n] = (1/J[n]) * D[n]^2 * M + // + // Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further. + // + // Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value + // of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into + // its constituent axes for execution the jerk for that axis will be at it's maximum value. + + float C; // contribution term. C = T * a + float maxC = 0; + float recip_L2 = 1/length_square; + + for (uint8_t axis=0; axis 0) { // You cannot use the fp_XXX comparisons here! + bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero) + C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's positive + if (C > maxC) { + maxC = C; + bf->jerk_axis = axis; // also needed for junction vmax calculation + } + } + } + // set up and pre-compute the jerk terms needed for this round of planning + bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale the jerk + + if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta + mm.jerk = bf->jerk; // used before this point next time around + mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning + mm.cbrt_jerk = cbrt(bf->jerk); + } + bf->recip_jerk = mm.recip_jerk; + bf->cbrt_jerk = mm.cbrt_jerk; + + // finish up the current block variables + if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed + bf->replannable = true; + exact_stop = 8675309; // an arbitrarily large floating point number + } + bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested + junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); + bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); + bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); + bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); + bf->braking_velocity = bf->delta_vmax; + + // Note: these next lines must remain in exact order. Position must update before committing the buffer. + _plan_block_list(bf, &mr_flag); // replan block list + copy_vector(mm.position, bf->gm.target); // set the planner position + mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update) + return STAT_OK; } /***** ALINE HELPERS ***** @@ -261,478 +261,478 @@ stat_t mp_aline(GCodeState_t *gm_in) /* * _calc_move_times() - compute optimal and minimum move times into the gcode_state * - * "Minimum time" is the fastest the move can be performed given the velocity constraints on each - * participating axis - regardless of the feed rate requested. The minimum time is the time limited - * by the rate-limiting axis. The minimum time is needed to compute the optimal time and is - * recorded for possible feed override computation.. + * "Minimum time" is the fastest the move can be performed given the velocity constraints on each + * participating axis - regardless of the feed rate requested. The minimum time is the time limited + * by the rate-limiting axis. The minimum time is needed to compute the optimal time and is + * recorded for possible feed override computation.. * - * "Optimal time" is either the time resulting from the requested feed rate or the minimum time if - * the requested feed rate is not achievable. Optimal times for traverses are always the minimum time. + * "Optimal time" is either the time resulting from the requested feed rate or the minimum time if + * the requested feed rate is not achievable. Optimal times for traverses are always the minimum time. * - * The gcode state must have targets set prior by having cm_set_target(). Axis modes are taken into - * account by this. + * The gcode state must have targets set prior by having cm_set_target(). Axis modes are taken into + * account by this. * - * The following times are compared and the longest is returned: - * - G93 inverse time (if G93 is active) - * - time for coordinated move at requested feed rate - * - time that the slowest axis would require for the move + * 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 + * 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. + * 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 + // gms = Gcode model state { - float inv_time=0; // inverse time if doing a feed in G93 mode - float xyz_time=0; // coordinated move linear part at requested feed rate - float abc_time=0; // coordinated move rotary part at requested feed rate - 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) { - inv_time = gms->feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_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 = AXIS_X; 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); + float inv_time=0; // inverse time if doing a feed in G93 mode + float xyz_time=0; // coordinated move linear part at requested feed rate + float abc_time=0; // coordinated move rotary part at requested feed rate + 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) { + inv_time = gms->feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_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 = AXIS_X; 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); } /* _plan_block_list() - plans the entire block list * - * The block list is the circular buffer of planner buffers (bf's). The block currently - * being planned is the "bf" block. The "first block" is the next block to execute; - * queued immediately behind the currently executing block, aka the "running" block. - * In some cases there is no first block because the list is empty or there is only - * one block and it is already running. + * The block list is the circular buffer of planner buffers (bf's). The block currently + * being planned is the "bf" block. The "first block" is the next block to execute; + * queued immediately behind the currently executing block, aka the "running" block. + * In some cases there is no first block because the list is empty or there is only + * one block and it is already running. * - * If blocks following the first block are already optimally planned (non replannable) - * the first block that is not optimally planned becomes the effective first block. + * If blocks following the first block are already optimally planned (non replannable) + * the first block that is not optimally planned becomes the effective first block. * - * _plan_block_list() plans all blocks between and including the (effective) first block - * and the bf. It sets entry, exit and cruise v's from vmax's then calls trapezoid generation. + * _plan_block_list() plans all blocks between and including the (effective) first block + * and the bf. It sets entry, exit and cruise v's from vmax's then calls trapezoid generation. * - * Variables that must be provided in the mpBuffers that will be processed: + * Variables that must be provided in the mpBuffers that will be processed: * - * bf (function arg) - end of block list (last block in time) - * bf->replannable - start of block list set by last FALSE value [Note 1] - * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should be set to - * length=0, entry_vmax=0 and exit_vmax=0 and are treated - * as a momentary stop (plan to zero and from zero). + * bf (function arg) - end of block list (last block in time) + * bf->replannable - start of block list set by last FALSE value [Note 1] + * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should be set to + * length=0, entry_vmax=0 and exit_vmax=0 and are treated + * as a momentary stop (plan to zero and from zero). * - * bf->length - provides block length - * bf->entry_vmax - used during forward planning to set entry velocity - * bf->cruise_vmax - used during forward planning to set cruise velocity - * bf->exit_vmax - used during forward planning to set exit velocity - * bf->delta_vmax - used during forward planning to set exit velocity + * bf->length - provides block length + * bf->entry_vmax - used during forward planning to set entry velocity + * bf->cruise_vmax - used during forward planning to set cruise velocity + * bf->exit_vmax - used during forward planning to set exit velocity + * bf->delta_vmax - used during forward planning to set exit velocity * - * bf->recip_jerk - used during trapezoid generation - * bf->cbrt_jerk - used during trapezoid generation + * bf->recip_jerk - used during trapezoid generation + * bf->cbrt_jerk - used during trapezoid generation * - * Variables that will be set during processing: + * Variables that will be set during processing: * - * bf->replannable - set if the block becomes optimally planned + * bf->replannable - set if the block becomes optimally planned * - * bf->braking_velocity - set during backward planning - * bf->entry_velocity - set during forward planning - * bf->cruise_velocity - set during forward planning - * bf->exit_velocity - set during forward planning + * bf->braking_velocity - set during backward planning + * bf->entry_velocity - set during forward planning + * bf->cruise_velocity - set during forward planning + * bf->exit_velocity - set during forward planning * - * bf->head_length - set during trapezoid generation - * bf->body_length - set during trapezoid generation - * bf->tail_length - set during trapezoid generation + * bf->head_length - set during trapezoid generation + * bf->body_length - set during trapezoid generation + * bf->tail_length - set during trapezoid generation * - * Variables that are ignored but here's what you would expect them to be: - * bf->move_state - NEW for all blocks but the earliest - * bf->target[] - block target position - * bf->unit[] - block unit vector - * bf->time - gets set later - * bf->jerk - source of the other jerk variables. Used in mr. + * Variables that are ignored but here's what you would expect them to be: + * bf->move_state - NEW for all blocks but the earliest + * bf->target[] - block target position + * bf->unit[] - block unit vector + * bf->time - gets set later + * bf->jerk - source of the other jerk variables. Used in mr. */ /* Notes: - * [1] Whether or not a block is planned is controlled by the bf->replannable - * setting (set TRUE if it should be). Replan flags are checked during the - * backwards pass and prune the replan list to include only the the latest - * blocks that require planning - * - * In normal operation the first block (currently running block) is not - * replanned, but may be for feedholds and feed overrides. In these cases - * the prep routines modify the contents of the mr buffer and re-shuffle - * the block list, re-enlisting the current bf buffer with new parameters. - * These routines also set all blocks in the list to be replannable so the - * list can be recomputed regardless of exact stops and previous replanning - * optimizations. - * - * [2] The mr_flag is used to tell replan to account for mr buffer's exit velocity (Vx) - * mr's Vx is always found in the provided bf buffer. Used to replan feedholds + * [1] Whether or not a block is planned is controlled by the bf->replannable + * setting (set TRUE if it should be). Replan flags are checked during the + * backwards pass and prune the replan list to include only the the latest + * blocks that require planning + * + * In normal operation the first block (currently running block) is not + * replanned, but may be for feedholds and feed overrides. In these cases + * the prep routines modify the contents of the mr buffer and re-shuffle + * the block list, re-enlisting the current bf buffer with new parameters. + * These routines also set all blocks in the list to be replannable so the + * list can be recomputed regardless of exact stops and previous replanning + * optimizations. + * + * [2] The mr_flag is used to tell replan to account for mr buffer's exit velocity (Vx) + * mr's Vx is always found in the provided bf buffer. Used to replan feedholds */ static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { - mpBuf_t *bp = bf; - - // Backward planning pass. Find first block and update the braking velocities. - // At the end *bp points to the buffer before the first block. - while ((bp = mp_get_prev_buffer(bp)) != bf) { - if (bp->replannable == false) { break; } - bp->braking_velocity = min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax; - } - - // forward planning pass - recomputes trapezoids in the list from the first block to the bf block. - while ((bp = mp_get_next_buffer(bp)) != bf) { - if ((bp->pv == bf) || (*mr_flag == true)) { - bp->entry_velocity = bp->entry_vmax; // first block in the list - *mr_flag = false; - } else { - bp->entry_velocity = bp->pv->exit_velocity; // other blocks in the list - } - bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = min4( bp->exit_vmax, - bp->nx->entry_vmax, - bp->nx->braking_velocity, - (bp->entry_velocity + bp->delta_vmax) ); - - mp_calculate_trapezoid(bp); - - // test for optimally planned trapezoids - only need to check various exit conditions - if ( ( (fp_EQ(bp->exit_velocity, bp->exit_vmax)) || - (fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) ) || - ( (bp->pv->replannable == false) && - (fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))) ) ) { - bp->replannable = false; - } - } - // finish up the last block move - bp->entry_velocity = bp->pv->exit_velocity; - bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = 0; - mp_calculate_trapezoid(bp); + mpBuf_t *bp = bf; + + // Backward planning pass. Find first block and update the braking velocities. + // At the end *bp points to the buffer before the first block. + while ((bp = mp_get_prev_buffer(bp)) != bf) { + if (bp->replannable == false) { break; } + bp->braking_velocity = min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax; + } + + // forward planning pass - recomputes trapezoids in the list from the first block to the bf block. + while ((bp = mp_get_next_buffer(bp)) != bf) { + if ((bp->pv == bf) || (*mr_flag == true)) { + bp->entry_velocity = bp->entry_vmax; // first block in the list + *mr_flag = false; + } else { + bp->entry_velocity = bp->pv->exit_velocity; // other blocks in the list + } + bp->cruise_velocity = bp->cruise_vmax; + bp->exit_velocity = min4( bp->exit_vmax, + bp->nx->entry_vmax, + bp->nx->braking_velocity, + (bp->entry_velocity + bp->delta_vmax) ); + + mp_calculate_trapezoid(bp); + + // test for optimally planned trapezoids - only need to check various exit conditions + if ( ( (fp_EQ(bp->exit_velocity, bp->exit_vmax)) || + (fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) ) || + ( (bp->pv->replannable == false) && + (fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))) ) ) { + bp->replannable = false; + } + } + // finish up the last block move + bp->entry_velocity = bp->pv->exit_velocity; + bp->cruise_velocity = bp->cruise_vmax; + bp->exit_velocity = 0; + mp_calculate_trapezoid(bp); } /* - * _reset_replannable_list() - resets all blocks in the planning list to be replannable + * _reset_replannable_list() - resets all blocks in the planning list to be replannable */ static void _reset_replannable_list() { - mpBuf_t *bf = mp_get_first_buffer(); - if (bf == NULL) return; - mpBuf_t *bp = bf; - do { - bp->replannable = true; - } while (((bp = mp_get_next_buffer(bp)) != bf) && (bp->move_state != MOVE_OFF)); + mpBuf_t *bf = mp_get_first_buffer(); + if (bf == 0) return; + mpBuf_t *bp = bf; + do { + bp->replannable = true; + } while (((bp = mp_get_next_buffer(bp)) != bf) && (bp->move_state != MOVE_OFF)); } /* * _get_junction_vmax() - 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. + * 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) + * 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). + * 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)). + * 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). + * Most of these calculations are already done in the planner. To remove the acos() + * and sin() computations, use the trig half angle identity: + * sin(theta/2) = +/- sqrt((1-cos(theta))/2). * - * For our applications, this should always be positive. Now just plug the equations into - * the centripetal acceleration equation: v_c = sqrt(a_max*R). You'll see that there are - * only two sqrt computations and no sine/cosines." + * For our applications, this should always be positive. Now just plug the equations into + * the centripetal acceleration equation: v_c = sqrt(a_max*R). You'll see that there are + * only two sqrt computations and no sine/cosines." * - * How to compute the radius using brute-force trig: - * float theta = acos(costheta); - * float radius = delta * sin(theta/2)/(1-sin(theta/2)); + * 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 + * 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); -// printf ("v:%f\n", velocity); //+++++ - return (velocity); + 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); +// printf ("v:%f\n", velocity); //+++++ + return velocity; } /************************************************************************* * feedholds - functions for performing holds * * mp_plan_hold_callback() - replan block list to execute hold - * mp_end_hold() - release the hold and restart block list + * mp_end_hold() - release the hold and restart block list * - * Feedhold is executed as cm.hold_state transitions executed inside - * _exec_aline() and main loop callbacks to these functions: - * mp_plan_hold_callback() and mp_end_hold(). + * Feedhold is executed as cm.hold_state transitions executed inside + * _exec_aline() and main loop callbacks to these functions: + * mp_plan_hold_callback() and mp_end_hold(). */ -/* Holds work like this: - * - * - Hold is asserted by calling cm_feedhold() (usually invoked via a ! char) - * If hold_state is OFF and motion_state is RUNning it sets - * hold_state to SYNC and motion_state to HOLD. - * - * - Hold state == SYNC tells the aline exec routine to execute the next aline - * segment then set hold_state to PLAN. This gives the planner sufficient - * time to replan the block list for the hold before the next aline segment - * needs to be processed. - * - * - Hold state == PLAN tells the planner to replan the mr buffer, the current - * run buffer (bf), and any subsequent bf buffers as necessary to execute a - * hold. Hold planning replans the planner buffer queue down to zero and then - * back up from zero. Hold state is set to DECEL when planning is complete. - * - * - Hold state == DECEL persists until the aline execution runs to zero - * velocity, at which point hold state transitions to HOLD. - * - * - Hold state == HOLD persists until the cycle is restarted. A cycle start - * is an asynchronous event that sets the cycle_start_flag TRUE. It can - * occur any time after the hold is requested - either before or after - * motion stops. - * - * - mp_end_hold() is executed from cm_feedhold_sequencing_callback() once the - * hold state == HOLD and a cycle_start has been requested.This sets the hold - * state to OFF which enables _exec_aline() to continue processing. Move - * execution begins with the first buffer after the hold. - * - * Terms used: - * - mr is the runtime buffer. It was initially loaded from the bf buffer - * - bp+0 is the "companion" bf buffer to the mr buffer. - * - bp+1 is the bf buffer following bp+0. This runs through bp+N - * - bp (by itself) just refers to the current buffer being adjusted / replanned - * - * Details: Planning re-uses bp+0 as an "extra" buffer. Normally bp+0 is returned - * to the buffer pool as it is redundant once mr is loaded. Use the extra - * buffer to split the move in two where the hold decelerates to zero. Use - * one buffer to go to zero, the other to replan up from zero. All buffers past - * that point are unaffected other than that they need to be replanned for velocity. - * - * Note: There are multiple opportunities for more efficient organization of - * code in this module, but the code is so complicated I just left it - * organized for clarity and hoped for the best from compiler optimization. +/* Holds work like this: + * + * - Hold is asserted by calling cm_feedhold() (usually invoked via a ! char) + * If hold_state is OFF and motion_state is RUNning it sets + * hold_state to SYNC and motion_state to HOLD. + * + * - Hold state == SYNC tells the aline exec routine to execute the next aline + * segment then set hold_state to PLAN. This gives the planner sufficient + * time to replan the block list for the hold before the next aline segment + * needs to be processed. + * + * - Hold state == PLAN tells the planner to replan the mr buffer, the current + * run buffer (bf), and any subsequent bf buffers as necessary to execute a + * hold. Hold planning replans the planner buffer queue down to zero and then + * back up from zero. Hold state is set to DECEL when planning is complete. + * + * - Hold state == DECEL persists until the aline execution runs to zero + * velocity, at which point hold state transitions to HOLD. + * + * - Hold state == HOLD persists until the cycle is restarted. A cycle start + * is an asynchronous event that sets the cycle_start_flag TRUE. It can + * occur any time after the hold is requested - either before or after + * motion stops. + * + * - mp_end_hold() is executed from cm_feedhold_sequencing_callback() once the + * hold state == HOLD and a cycle_start has been requested.This sets the hold + * state to OFF which enables _exec_aline() to continue processing. Move + * execution begins with the first buffer after the hold. + * + * Terms used: + * - mr is the runtime buffer. It was initially loaded from the bf buffer + * - bp+0 is the "companion" bf buffer to the mr buffer. + * - bp+1 is the bf buffer following bp+0. This runs through bp+N + * - bp (by itself) just refers to the current buffer being adjusted / replanned + * + * Details: Planning re-uses bp+0 as an "extra" buffer. Normally bp+0 is returned + * to the buffer pool as it is redundant once mr is loaded. Use the extra + * buffer to split the move in two where the hold decelerates to zero. Use + * one buffer to go to zero, the other to replan up from zero. All buffers past + * that point are unaffected other than that they need to be replanned for velocity. + * + * Note: There are multiple opportunities for more efficient organization of + * code in this module, but the code is so complicated I just left it + * organized for clarity and hoped for the best from compiler optimization. */ static float _compute_next_segment_velocity() { - if (mr.section == SECTION_BODY) return (mr.segment_velocity); + if (mr.section == SECTION_BODY) return mr.segment_velocity; #ifdef __JERK_EXEC - return (mr.segment_velocity); // an approximation + return mr.segment_velocity; // an approximation #else - return (mr.segment_velocity + mr.forward_diff_5); + return mr.segment_velocity + mr.forward_diff_5; #endif } stat_t mp_plan_hold_callback() { - if (cm.hold_state != FEEDHOLD_PLAN) - return (STAT_NOOP); // not planning a feedhold + if (cm.hold_state != FEEDHOLD_PLAN) + return STAT_NOOP; // not planning a feedhold - mpBuf_t *bp; // working buffer pointer - if ((bp = mp_get_run_buffer()) == NULL) - return (STAT_NOOP); // Oops! nothing's running + mpBuf_t *bp; // working buffer pointer + if ((bp = mp_get_run_buffer()) == 0) + return STAT_NOOP; // Oops! nothing's running - uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx - float mr_available_length; // available length left in mr buffer for deceleration - float braking_velocity; // velocity left to shed to brake to zero - float braking_length; // distance required to brake to zero from braking_velocity + uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx + float mr_available_length; // available length left in mr buffer for deceleration + float braking_velocity; // velocity left to shed to brake to zero + float braking_length; // distance required to brake to zero from braking_velocity - // examine and process mr buffer - mr_available_length = get_axis_vector_length(mr.target, mr.position); + // examine and process mr buffer + mr_available_length = get_axis_vector_length(mr.target, mr.position); // compute next_segment velocity braking_velocity = _compute_next_segment_velocity(); - braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here - - // Hack to prevent Case 2 moves for perfect-fit decels. Happens in homing situations - // The real fix: The braking velocity cannot simply be the mr.segment_velocity as this - // is the velocity of the last segment, not the one that's going to be executed next. - // The braking_velocity needs to be the velocity of the next segment that has not yet - // been computed. In the mean time, this hack will work. - if ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) { - braking_length = mr_available_length; - } - - // Case 1: deceleration fits entirely into the length remaining in mr buffer - if (braking_length <= mr_available_length) { - // set mr to a tail to perform the deceleration - mr.exit_velocity = 0; - mr.tail_length = braking_length; - mr.cruise_velocity = braking_velocity; - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - - // re-use bp+0 to be the hold point and to run the remaining block length - bp->length = mr_available_length - braking_length; - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->entry_vmax = 0; // set bp+0 as hold point - bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer - - _reset_replannable_list(); // make it replan all the blocks - _plan_block_list(mp_get_last_buffer(), &mr_flag); - cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit - return (STAT_OK); - } - - // Case 2: deceleration exceeds length remaining in mr buffer - // First, replan mr to minimum (but non-zero) exit velocity - - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - mr.tail_length = mr_available_length; - mr.cruise_velocity = braking_velocity; - mr.exit_velocity = braking_velocity - mp_get_target_velocity(0, mr_available_length, bp); - - // Find the point where deceleration reaches zero. This could span multiple buffers. - braking_velocity = mr.exit_velocity; // adjust braking velocity downward - bp->move_state = MOVE_NEW; // tell _exec to re-use buffer - for (uint8_t i=0; inx); // copy bp+1 into bp+0 (and onward...) - if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - bp->entry_vmax = braking_velocity; // velocity we need to shed - braking_length = mp_get_target_length(braking_velocity, 0, bp); - - if (braking_length > bp->length) { // decel does not fit in bp buffer - bp->exit_vmax = braking_velocity - mp_get_target_velocity(0, bp->length, bp); - braking_velocity = bp->exit_vmax; // braking velocity for next buffer - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - break; - } - // Deceleration now fits in the current bp buffer - // Plan the first buffer of the pair as the decel, the second as the accel - bp->length = braking_length; - bp->exit_vmax = 0; - - bp = mp_get_next_buffer(bp); // point to the acceleration buffer - bp->entry_vmax = 0; - bp->length -= braking_length; // the buffers were identical (and hence their lengths) - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->exit_vmax = bp->delta_vmax; - - _reset_replannable_list(); // make it replan all the blocks - _plan_block_list(mp_get_last_buffer(), &mr_flag); - cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit - return (STAT_OK); + braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here + + // Hack to prevent Case 2 moves for perfect-fit decels. Happens in homing situations + // The real fix: The braking velocity cannot simply be the mr.segment_velocity as this + // is the velocity of the last segment, not the one that's going to be executed next. + // The braking_velocity needs to be the velocity of the next segment that has not yet + // been computed. In the mean time, this hack will work. + if ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) { + braking_length = mr_available_length; + } + + // Case 1: deceleration fits entirely into the length remaining in mr buffer + if (braking_length <= mr_available_length) { + // set mr to a tail to perform the deceleration + mr.exit_velocity = 0; + mr.tail_length = braking_length; + mr.cruise_velocity = braking_velocity; + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + + // re-use bp+0 to be the hold point and to run the remaining block length + bp->length = mr_available_length - braking_length; + bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); + bp->entry_vmax = 0; // set bp+0 as hold point + bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer + + _reset_replannable_list(); // make it replan all the blocks + _plan_block_list(mp_get_last_buffer(), &mr_flag); + cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + return STAT_OK; + } + + // Case 2: deceleration exceeds length remaining in mr buffer + // First, replan mr to minimum (but non-zero) exit velocity + + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + mr.tail_length = mr_available_length; + mr.cruise_velocity = braking_velocity; + mr.exit_velocity = braking_velocity - mp_get_target_velocity(0, mr_available_length, bp); + + // Find the point where deceleration reaches zero. This could span multiple buffers. + braking_velocity = mr.exit_velocity; // adjust braking velocity downward + bp->move_state = MOVE_NEW; // tell _exec to re-use buffer + for (uint8_t i=0; inx); // copy bp+1 into bp+0 (and onward...) + if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers + bp = mp_get_next_buffer(bp); // point to next buffer + continue; + } + bp->entry_vmax = braking_velocity; // velocity we need to shed + braking_length = mp_get_target_length(braking_velocity, 0, bp); + + if (braking_length > bp->length) { // decel does not fit in bp buffer + bp->exit_vmax = braking_velocity - mp_get_target_velocity(0, bp->length, bp); + braking_velocity = bp->exit_vmax; // braking velocity for next buffer + bp = mp_get_next_buffer(bp); // point to next buffer + continue; + } + break; + } + // Deceleration now fits in the current bp buffer + // Plan the first buffer of the pair as the decel, the second as the accel + bp->length = braking_length; + bp->exit_vmax = 0; + + bp = mp_get_next_buffer(bp); // point to the acceleration buffer + bp->entry_vmax = 0; + bp->length -= braking_length; // the buffers were identical (and hence their lengths) + bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); + bp->exit_vmax = bp->delta_vmax; + + _reset_replannable_list(); // make it replan all the blocks + _plan_block_list(mp_get_last_buffer(), &mr_flag); + cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + return STAT_OK; } /* @@ -740,15 +740,15 @@ stat_t mp_plan_hold_callback() */ stat_t mp_end_hold() { - if (cm.hold_state == FEEDHOLD_END_HOLD) { - cm.hold_state = FEEDHOLD_OFF; - mpBuf_t *bf; - if ((bf = mp_get_run_buffer()) == NULL) { // NULL means nothing's running - cm_set_motion_state(MOTION_STOP); - return (STAT_NOOP); - } - cm.motion_state = MOTION_RUN; - st_request_exec_move(); // restart the steppers - } - return (STAT_OK); + if (cm.hold_state == FEEDHOLD_END_HOLD) { + cm.hold_state = FEEDHOLD_OFF; + mpBuf_t *bf; + if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running + cm_set_motion_state(MOTION_STOP); + return STAT_NOOP; + } + cm.motion_state = MOTION_RUN; + st_request_exec_move(); // restart the steppers + } + return STAT_OK; } diff --git a/src/plan_zoid.c b/src/plan_zoid.c index 7008c9d..f477438 100755 --- a/src/plan_zoid.c +++ b/src/plan_zoid.c @@ -35,84 +35,84 @@ /* * mp_calculate_trapezoid() - calculate trapezoid parameters * - * This rather brute-force and long-ish function sets section lengths and velocities - * based on the line length and velocities requested. It modifies the incoming - * bf buffer and returns accurate head, body and tail lengths, and accurate or - * reasonably approximate velocities. We care about accuracy on lengths, less - * so for velocity (as long as velocity err's on the side of too slow). - * - * Note: We need the velocities to be set even for zero-length sections - * (Note: sections, not moves) so we can compute entry and exits for adjacent sections. - * - * Inputs used are: - * bf->length - actual block length (length is never changed) - * bf->entry_velocity - requested Ve (entry velocity is never changed) - * bf->cruise_velocity - requested Vt (is often changed) - * bf->exit_velocity - requested Vx (may be changed for degenerate cases) - * bf->cruise_vmax - used in some comparisons - * bf->delta_vmax - used to degrade velocity of pathologically short blocks - * - * Variables that may be set/updated are: - * bf->entry_velocity - requested Ve - * bf->cruise_velocity - requested Vt - * bf->exit_velocity - requested Vx - * bf->head_length - bf->length allocated to head - * bf->body_length - bf->length allocated to body - * bf->tail_length - bf->length allocated to tail - * - * Note: The following conditions must be met on entry: - * bf->length must be non-zero (filter these out upstream) - * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity + * This rather brute-force and long-ish function sets section lengths and velocities + * based on the line length and velocities requested. It modifies the incoming + * bf buffer and returns accurate head, body and tail lengths, and accurate or + * reasonably approximate velocities. We care about accuracy on lengths, less + * so for velocity (as long as velocity err's on the side of too slow). + * + * Note: We need the velocities to be set even for zero-length sections + * (Note: sections, not moves) so we can compute entry and exits for adjacent sections. + * + * Inputs used are: + * bf->length - actual block length (length is never changed) + * bf->entry_velocity - requested Ve (entry velocity is never changed) + * bf->cruise_velocity - requested Vt (is often changed) + * bf->exit_velocity - requested Vx (may be changed for degenerate cases) + * bf->cruise_vmax - used in some comparisons + * bf->delta_vmax - used to degrade velocity of pathologically short blocks + * + * Variables that may be set/updated are: + * bf->entry_velocity - requested Ve + * bf->cruise_velocity - requested Vt + * bf->exit_velocity - requested Vx + * bf->head_length - bf->length allocated to head + * bf->body_length - bf->length allocated to body + * bf->tail_length - bf->length allocated to tail + * + * Note: The following conditions must be met on entry: + * bf->length must be non-zero (filter these out upstream) + * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity */ -/* Classes of moves: - * - * Requested-Fit - The move has sufficient length to achieve the target velocity - * (cruise velocity). I.e: it will accommodate the acceleration / deceleration - * profile in the given length. - * - * Rate-Limited-Fit - The move does not have sufficient length to achieve target - * velocity. In this case the cruise velocity will be set lower than the requested - * velocity (incoming bf->cruise_velocity). The entry and exit velocities are satisfied. - * - * Degraded-Fit - The move does not have sufficient length to transition from - * the entry velocity to the exit velocity in the available length. These - * velocities are not negotiable, so a degraded solution is found. - * - * In worst cases the move cannot be executed as the required execution time is - * less than the minimum segment time. The first degradation is to reduce the - * move to a body-only segment with an average velocity. If that still doesn't - * fit then the move velocity is reduced so it fits into a minimum segment. - * This will reduce the velocities in that region of the planner buffer as the - * moves are replanned to that worst-case move. - * - * Various cases handled (H=head, B=body, T=tail) - * - * Requested-Fit cases - * HBT VeVx sufficient length exists for all parts (corner case: HBT') - * HB VeVx enter at full speed and decelerate (corner case: T') - * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric - * H VeVx perfect fit T (common, results from planning) - * B Ve=Vt=Vx Velocities are close to each other and within matching tolerance - * - * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot - * HT (Ve=Vx)Vx Ve is degraded (velocity step). Vx is met - * B" line is very short but drawable; is treated as a body only - * F force fit: This block is slowed down until it can be executed +/* Classes of moves: + * + * Requested-Fit - The move has sufficient length to achieve the target velocity + * (cruise velocity). I.e: it will accommodate the acceleration / deceleration + * profile in the given length. + * + * Rate-Limited-Fit - The move does not have sufficient length to achieve target + * velocity. In this case the cruise velocity will be set lower than the requested + * velocity (incoming bf->cruise_velocity). The entry and exit velocities are satisfied. + * + * Degraded-Fit - The move does not have sufficient length to transition from + * the entry velocity to the exit velocity in the available length. These + * velocities are not negotiable, so a degraded solution is found. + * + * In worst cases the move cannot be executed as the required execution time is + * less than the minimum segment time. The first degradation is to reduce the + * move to a body-only segment with an average velocity. If that still doesn't + * fit then the move velocity is reduced so it fits into a minimum segment. + * This will reduce the velocities in that region of the planner buffer as the + * moves are replanned to that worst-case move. + * + * Various cases handled (H=head, B=body, T=tail) + * + * Requested-Fit cases + * HBT VeVx sufficient length exists for all parts (corner case: HBT') + * HB VeVx enter at full speed and decelerate (corner case: T') + * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric + * H VeVx perfect fit T (common, results from planning) + * B Ve=Vt=Vx Velocities are close to each other and within matching tolerance + * + * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot + * HT (Ve=Vx)Vx Ve is degraded (velocity step). Vx is met + * B" line is very short but drawable; is treated as a body only + * F force fit: This block is slowed down until it can be executed */ -/* NOTE: The order of the cases/tests in the code is pretty important. Start with the - * shortest cases first and work up. Not only does this simplify the order of the tests, - * but it reduces execution time when you need it most - when tons of pathologically - * short Gcode blocks are being thrown at you. +/* NOTE: The order of the cases/tests in the code is pretty important. Start with the + * shortest cases first and work up. Not only does this simplify the order of the tests, + * but it reduces execution time when you need it most - when tons of pathologically + * short Gcode blocks are being thrown at you. */ // The minimum lengths are dynamic and depend on the velocity @@ -124,225 +124,225 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { - //******************************************** - //******************************************** - //** RULE #1 of mp_calculate_trapezoid() ** - //** DON'T CHANGE bf->length ** - //******************************************** - //******************************************** + //******************************************** + //******************************************** + //** RULE #1 of mp_calculate_trapezoid() ** + //** DON'T CHANGE bf->length ** + //******************************************** + //******************************************** - // F case: Block is too short - run time < minimum segment time - // Force block into a single segment body with limited velocities - // Accept the entry velocity, limit the cruise, and go for the best exit velocity - // you can get given the delta_vmax (maximum velocity slew) supportable. + // F case: Block is too short - run time < minimum segment time + // Force block into a single segment body with limited velocities + // Accept the entry velocity, limit the cruise, and go for the best exit velocity + // you can get given the delta_vmax (maximum velocity slew) supportable. - bf->naiive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average + bf->naiive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average - if (bf->naiive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { - bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; - bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - // We are violating the jerk value but since it's a single segment move we don't use it. - return; - } + if (bf->naiive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { + bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; + bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; + // We are violating the jerk value but since it's a single segment move we don't use it. + return; + } - // B" case: Block is short, but fits into a single body segment + // B" case: Block is short, but fits into a single body segment - if (bf->naiive_move_time <= NOM_SEGMENT_TIME) { - bf->entry_velocity = bf->pv->exit_velocity; - if (fp_NOT_ZERO(bf->entry_velocity)) { - bf->cruise_velocity = bf->entry_velocity; - bf->exit_velocity = bf->entry_velocity; - } else { - bf->cruise_velocity = bf->delta_vmax / 2; - bf->exit_velocity = bf->delta_vmax; - } - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - // We are violating the jerk value but since it's a single segment move we don't use it. - return; - } + if (bf->naiive_move_time <= NOM_SEGMENT_TIME) { + bf->entry_velocity = bf->pv->exit_velocity; + if (fp_NOT_ZERO(bf->entry_velocity)) { + bf->cruise_velocity = bf->entry_velocity; + bf->exit_velocity = bf->entry_velocity; + } else { + bf->cruise_velocity = bf->delta_vmax / 2; + bf->exit_velocity = bf->delta_vmax; + } + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; + // We are violating the jerk value but since it's a single segment move we don't use it. + return; + } - // B case: Velocities all match (or close enough) - // This occurs frequently in normal gcode files with lots of short lines - // This case is not really necessary, but saves lots of processing time + // B case: Velocities all match (or close enough) + // This occurs frequently in normal gcode files with lots of short lines + // This case is not really necessary, but saves lots of processing time - if (((bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) && - ((bf->cruise_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE)) { - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - return; - } + if (((bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) && + ((bf->cruise_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE)) { + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; + return; + } - // Head-only and tail-only short-line cases - // H" and T" degraded-fit cases - // H' and T' requested-fit cases where the body residual is less than MIN_BODY_LENGTH + // Head-only and tail-only short-line cases + // H" and T" degraded-fit cases + // H' and T' requested-fit cases where the body residual is less than MIN_BODY_LENGTH - bf->body_length = 0; - float minimum_length = mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf); - if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { // head-only & tail-only cases + bf->body_length = 0; + float minimum_length = mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf); + if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { // head-only & tail-only cases - if (bf->entry_velocity > bf->exit_velocity) { // tail-only cases (short decelerations) - if (bf->length < minimum_length) { // T" (degraded case) - bf->entry_velocity = mp_get_target_velocity(bf->exit_velocity, bf->length, bf); - } - bf->cruise_velocity = bf->entry_velocity; - bf->tail_length = bf->length; - bf->head_length = 0; - return; - } + if (bf->entry_velocity > bf->exit_velocity) { // tail-only cases (short decelerations) + if (bf->length < minimum_length) { // T" (degraded case) + bf->entry_velocity = mp_get_target_velocity(bf->exit_velocity, bf->length, bf); + } + bf->cruise_velocity = bf->entry_velocity; + bf->tail_length = bf->length; + bf->head_length = 0; + return; + } - if (bf->entry_velocity < bf->exit_velocity) { // head-only cases (short accelerations) - if (bf->length < minimum_length) { // H" (degraded case) - bf->exit_velocity = mp_get_target_velocity(bf->entry_velocity, bf->length, bf); - } - bf->cruise_velocity = bf->exit_velocity; - bf->head_length = bf->length; - bf->tail_length = 0; - return; - } - } + if (bf->entry_velocity < bf->exit_velocity) { // head-only cases (short accelerations) + if (bf->length < minimum_length) { // H" (degraded case) + bf->exit_velocity = mp_get_target_velocity(bf->entry_velocity, bf->length, bf); + } + bf->cruise_velocity = bf->exit_velocity; + bf->head_length = bf->length; + bf->tail_length = 0; + return; + } + } - // Set head and tail lengths for evaluating the next cases - bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); - bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); - if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;} - if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;} + // Set head and tail lengths for evaluating the next cases + bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); + bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); + if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;} + if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;} - // Rate-limited HT and HT' cases - if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited + // Rate-limited HT and HT' cases + if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited - // Symmetric rate-limited case (HT) - if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) { - bf->head_length = bf->length/2; - bf->tail_length = bf->head_length; - bf->cruise_velocity = min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); + // Symmetric rate-limited case (HT) + if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) { + bf->head_length = bf->length/2; + bf->tail_length = bf->head_length; + bf->cruise_velocity = min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); - if (bf->head_length < MIN_HEAD_LENGTH) { - // Convert this to a body-only move - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; + if (bf->head_length < MIN_HEAD_LENGTH) { + // Convert this to a body-only move + bf->body_length = bf->length; + bf->head_length = 0; + bf->tail_length = 0; - // Average the entry speed and computed best cruise-speed - bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2; - bf->entry_velocity = bf->cruise_velocity; - bf->exit_velocity = bf->cruise_velocity; - } - return; - } + // Average the entry speed and computed best cruise-speed + bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2; + bf->entry_velocity = bf->cruise_velocity; + bf->exit_velocity = bf->cruise_velocity; + } + return; + } - // Asymmetric HT' rate-limited case. This is relatively expensive but it's not called very often - // iteration trap: uint8_t i=0; - // iteration trap: if (++i > TRAPEZOID_ITERATION_MAX) { fprintf_P(stderr,PSTR("_calculate_trapezoid() failed to converge"));} + // Asymmetric HT' rate-limited case. This is relatively expensive but it's not called very often + // iteration trap: uint8_t i=0; + // iteration trap: if (++i > TRAPEZOID_ITERATION_MAX) { fprintf_P(stderr,PSTR("_calculate_trapezoid() failed to converge"));} - float computed_velocity = bf->cruise_vmax; - do { - bf->cruise_velocity = computed_velocity; // initialize from previous iteration - bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); - bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); - if (bf->head_length > bf->tail_length) { - bf->head_length = (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length; - computed_velocity = mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf); - } else { - bf->tail_length = (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length; - computed_velocity = mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf); - } - // insert iteration trap here if needed - } while ((fabs(bf->cruise_velocity - computed_velocity) / computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT); + float computed_velocity = bf->cruise_vmax; + do { + bf->cruise_velocity = computed_velocity; // initialize from previous iteration + bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); + bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); + if (bf->head_length > bf->tail_length) { + bf->head_length = (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length; + computed_velocity = mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf); + } else { + bf->tail_length = (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length; + computed_velocity = mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf); + } + // insert iteration trap here if needed + } while ((fabs(bf->cruise_velocity - computed_velocity) / computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT); - // set velocity and clean up any parts that are too short - bf->cruise_velocity = computed_velocity; - bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); - bf->tail_length = bf->length - bf->head_length; - if (bf->head_length < MIN_HEAD_LENGTH) { - bf->tail_length = bf->length; // adjust the move to be all tail... - bf->head_length = 0; - } - if (bf->tail_length < MIN_TAIL_LENGTH) { - bf->head_length = bf->length; //...or all head - bf->tail_length = 0; - } - return; - } + // set velocity and clean up any parts that are too short + bf->cruise_velocity = computed_velocity; + bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); + bf->tail_length = bf->length - bf->head_length; + if (bf->head_length < MIN_HEAD_LENGTH) { + bf->tail_length = bf->length; // adjust the move to be all tail... + bf->head_length = 0; + } + if (bf->tail_length < MIN_TAIL_LENGTH) { + bf->head_length = bf->length; //...or all head + bf->tail_length = 0; + } + return; + } - // Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases - bf->body_length = bf->length - bf->head_length - bf->tail_length; + // Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases + bf->body_length = bf->length - bf->head_length - bf->tail_length; - // If a non-zero body is < minimum length distribute it to the head and/or tail - // This will generate small (acceptable) velocity errors in runtime execution - // but preserve correct distance, which is more important. - if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) { - if (fp_NOT_ZERO(bf->head_length)) { - if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT - bf->head_length += bf->body_length/2; - bf->tail_length += bf->body_length/2; - } else { // HB reduces to H - bf->head_length += bf->body_length; - } - } else { // BT reduces to T - bf->tail_length += bf->body_length; - } - bf->body_length = 0; + // If a non-zero body is < minimum length distribute it to the head and/or tail + // This will generate small (acceptable) velocity errors in runtime execution + // but preserve correct distance, which is more important. + if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) { + if (fp_NOT_ZERO(bf->head_length)) { + if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT + bf->head_length += bf->body_length/2; + bf->tail_length += bf->body_length/2; + } else { // HB reduces to H + bf->head_length += bf->body_length; + } + } else { // BT reduces to T + bf->tail_length += bf->body_length; + } + bf->body_length = 0; - // If the body is a standalone make the cruise velocity match the entry velocity - // This removes a potential velocity discontinuity at the expense of top speed - } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) { - bf->cruise_velocity = bf->entry_velocity; - } + // If the body is a standalone make the cruise velocity match the entry velocity + // This removes a potential velocity discontinuity at the expense of top speed + } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) { + bf->cruise_velocity = bf->entry_velocity; + } } /* - * mp_get_target_length() - derive accel/decel length from delta V and jerk + * mp_get_target_length() - derive accel/decel length from delta V and jerk * mp_get_target_velocity() - derive velocity achievable from delta V and length * - * This set of functions returns the fourth thing knowing the other three. + * This set of functions returns the fourth thing knowing the other three. * - * Jm = the given maximum jerk - * T = time of the entire move + * Jm = the given maximum jerk + * T = time of the entire move * Vi = initial velocity * Vf = final velocity - * T = 2*sqrt((Vt-Vi)/Jm) - * As = The acceleration at inflection point between convex and concave portions of the S-curve. - * As = (Jm*T)/2 + * T = 2*sqrt((Vt-Vi)/Jm) + * As = The acceleration at inflection point between convex and concave portions of the S-curve. + * As = (Jm*T)/2 * Ar = ramp acceleration - * Ar = As/2 = (Jm*T)/4 + * Ar = As/2 = (Jm*T)/4 * - * mp_get_target_length() is a convenient function for determining the optimal_length (L) - * of a line given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm). + * mp_get_target_length() is a convenient function for determining the optimal_length (L) + * of a line given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm). * - * The length (distance) equation is derived from: + * The length (distance) equation is derived from: * - * a) L = (Vf-Vi) * T - (Ar*T^2)/2 ... which becomes b) with substitutions for Ar and T - * b) L = (Vf-Vi) * 2*sqrt((Vf-Vi)/Jm) - (2*sqrt((Vf-Vi)/Jm) * (Vf-Vi))/2 - * c) L = (Vf-Vi)^(3/2) / sqrt(Jm) ...is an alternate form of b) (see Wolfram Alpha) - * c')L = (Vf-Vi) * sqrt((Vf-Vi)/Jm) ... second alternate form; requires Vf >= Vi + * a) L = (Vf-Vi) * T - (Ar*T^2)/2 ... which becomes b) with substitutions for Ar and T + * b) L = (Vf-Vi) * 2*sqrt((Vf-Vi)/Jm) - (2*sqrt((Vf-Vi)/Jm) * (Vf-Vi))/2 + * c) L = (Vf-Vi)^(3/2) / sqrt(Jm) ...is an alternate form of b) (see Wolfram Alpha) + * c')L = (Vf-Vi) * sqrt((Vf-Vi)/Jm) ... second alternate form; requires Vf >= Vi * - * Notes: Ar = (Jm*T)/4 Ar is ramp acceleration - * T = 2*sqrt((Vf-Vi)/Jm) T is time - * Assumes Vi, Vf and L are positive or zero - * Cannot assume Vf>=Vi due to rounding errors and use of PLANNER_VELOCITY_TOLERANCE - * necessitating the introduction of fabs() + * Notes: Ar = (Jm*T)/4 Ar is ramp acceleration + * T = 2*sqrt((Vf-Vi)/Jm) T is time + * Assumes Vi, Vf and L are positive or zero + * Cannot assume Vf>=Vi due to rounding errors and use of PLANNER_VELOCITY_TOLERANCE + * necessitating the introduction of fabs() * - * mp_get_target_velocity() is a convenient function for determining Vf target velocity for - * a given the initial velocity (Vi), length (L), and maximum jerk (Jm). - * Equation d) is b) solved for Vf. Equation e) is c) solved for Vf. Use e) (obviously) + * mp_get_target_velocity() is a convenient function for determining Vf target velocity for + * a given the initial velocity (Vi), length (L), and maximum jerk (Jm). + * Equation d) is b) solved for Vf. Equation e) is c) solved for Vf. Use e) (obviously) * - * d) Vf = (sqrt(L)*(L/sqrt(1/Jm))^(1/6)+(1/Jm)^(1/4)*Vi)/(1/Jm)^(1/4) - * e) Vf = L^(2/3) * Jm^(1/3) + Vi + * d) Vf = (sqrt(L)*(L/sqrt(1/Jm))^(1/6)+(1/Jm)^(1/4)*Vi)/(1/Jm)^(1/4) + * e) Vf = L^(2/3) * Jm^(1/3) + Vi * * FYI: Here's an expression that returns the jerk for a given deltaV and L: - * return(cube(deltaV / (pow(L, 0.66666666)))); + * return(cube(deltaV / (pow(L, 0.66666666)))); */ float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) { -// return (Vi + Vf) * sqrt(fabs(Vf - Vi) * bf->recip_jerk); // new formula - return (fabs(Vi-Vf) * sqrt(fabs(Vi-Vf) * bf->recip_jerk)); // old formula +// return Vi + Vf * sqrt(fabs(Vf - Vi) * bf->recip_jerk); // new formula + return fabs(Vi-Vf * sqrt(fabs(Vi-Vf) * bf->recip_jerk)); // old formula } /* Regarding mp_get_target_velocity: @@ -389,7 +389,7 @@ float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) * J'(x) = (2*Vi*x - Vi² + 3*x²) / L² */ -#define GET_VELOCITY_ITERATIONS 0 // must be 0, 1, or 2 +#define GET_VELOCITY_ITERATIONS 0 // must be 0, 1, or 2 float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf) { // 0 iterations (a reasonable estimate) diff --git a/src/planner.c b/src/planner.c index 3286a73..df5f394 100755 --- a/src/planner.c +++ b/src/planner.c @@ -27,29 +27,29 @@ */ /* --- Planner Notes ---- * - * The planner works below the canonical machine and above the motor mapping - * and stepper execution layers. A rudimentary multitasking capability is - * implemented for long-running commands such as lines, arcs, and dwells. - * These functions are coded as non-blocking continuations - which are simple - * state machines that are re-entered multiple times until a particular - * operation is complete. These functions have 2 parts - the initial call, - * which sets up the local context, and callbacks (continuations) that are - * called from the main loop (in controller.c). + * The planner works below the canonical machine and above the motor mapping + * and stepper execution layers. A rudimentary multitasking capability is + * implemented for long-running commands such as lines, arcs, and dwells. + * These functions are coded as non-blocking continuations - which are simple + * state machines that are re-entered multiple times until a particular + * operation is complete. These functions have 2 parts - the initial call, + * which sets up the local context, and callbacks (continuations) that are + * called from the main loop (in controller.c). * - * One important concept is isolation of the three layers of the data model - - * the Gcode model (gm), planner model (bf queue & mm), and runtime model (mr). - * These are designated as "model", "planner" and "runtime" in function names. + * One important concept is isolation of the three layers of the data model - + * the Gcode model (gm), planner model (bf queue & mm), and runtime model (mr). + * These are designated as "model", "planner" and "runtime" in function names. * - * The Gcode model is owned by the canonical machine and should only be accessed - * by cm_xxxx() functions. Data from the Gcode model is transferred to the planner - * by the mp_xxx() functions called by the canonical machine. + * The Gcode model is owned by the canonical machine and should only be accessed + * by cm_xxxx() functions. Data from the Gcode model is transferred to the planner + * by the mp_xxx() functions called by the canonical machine. * - * The planner should only use data in the planner model. When a move (block) - * is ready for execution the planner data is transferred to the runtime model, - * which should also be isolated. + * The planner should only use data in the planner model. When a move (block) + * is ready for execution the planner data is transferred to the runtime model, + * which should also be isolated. * - * Lower-level models should never use data from upper-level models as the data - * may have changed and lead to unpredictable results. + * Lower-level models should never use data from upper-level models as the data + * may have changed and lead to unpredictable results. */ #include "tinyg.h" #include "config.h" @@ -63,17 +63,17 @@ #include "util.h" // Allocate planner structures -mpBufferPool_t mb; // move buffer queue -mpMoveMasterSingleton_t mm; // context for line planning -mpMoveRuntimeSingleton_t mr; // context for line runtime +mpBufferPool_t mb; // move buffer queue +mpMoveMasterSingleton_t mm; // context for line planning +mpMoveRuntimeSingleton_t mr; // context for line runtime /* * Local Scope Data and Functions */ #define _bump(a) ((a, e.g. an M code) - * - cm_ function calls mp_queue_command which puts it in the planning queue (bf buffer). - * This involves setting some parameters and registering a callback to the - * execution function in the canonical machine - * - the planning queue gets to the function and calls _exec_command() - * - ...which puts a pointer to the bf buffer in the prep stratuc (st_pre) - * - When the runtime gets to the end of the current activity (sending steps, counting a dwell) - * if executes mp_runtime_command... - * - ...which uses the callback function in the bf and the saved parameters in the vectors - * - To finish up mp_runtime_command() needs to free the bf buffer + * How this works: + * - The command is called by the Gcode interpreter (cm_, e.g. an M code) + * - cm_ function calls mp_queue_command which puts it in the planning queue (bf buffer). + * This involves setting some parameters and registering a callback to the + * execution function in the canonical machine + * - the planning queue gets to the function and calls _exec_command() + * - ...which puts a pointer to the bf buffer in the prep stratuc (st_pre) + * - When the runtime gets to the end of the current activity (sending steps, counting a dwell) + * if executes mp_runtime_command... + * - ...which uses the callback function in the bf and the saved parameters in the vectors + * - To finish up mp_runtime_command() needs to free the bf buffer * - * Doing it this way instead of synchronizing on queue empty simplifies the - * handling of feedholds, feed overrides, buffer flushes, and thread blocking, - * and makes keeping the queue full much easier - therefore avoiding Q starvation + * Doing it this way instead of synchronizing on queue empty simplifies the + * handling of feedholds, feed overrides, buffer flushes, and thread blocking, + * and makes keeping the queue full much easier - therefore avoiding Q starvation */ void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, float *flag) { - mpBuf_t *bf; - - // Never supposed to fail as buffer availability was checked upstream in the controller - if ((bf = mp_get_write_buffer()) == NULL) { - cm_hard_alarm(STAT_BUFFER_FULL_FATAL); - return; - } - - bf->move_type = MOVE_TYPE_COMMAND; - bf->bf_func = _exec_command; // callback to planner queue exec function - bf->cm_func = cm_exec; // callback to canonical machine exec function - - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - bf->value_vector[axis] = value[axis]; - bf->flag_vector[axis] = flag[axis]; - } - mp_commit_write_buffer(MOVE_TYPE_COMMAND); // must be final operation before exit + mpBuf_t *bf; + + // Never supposed to fail as buffer availability was checked upstream in the controller + if ((bf = mp_get_write_buffer()) == 0) { + cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + return; + } + + bf->move_type = MOVE_TYPE_COMMAND; + bf->bf_func = _exec_command; // callback to planner queue exec function + bf->cm_func = cm_exec; // callback to canonical machine exec function + + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + bf->value_vector[axis] = value[axis]; + bf->flag_vector[axis] = flag[axis]; + } + mp_commit_write_buffer(MOVE_TYPE_COMMAND); // must be final operation before exit } static stat_t _exec_command(mpBuf_t *bf) { - st_prep_command(bf); - return (STAT_OK); + st_prep_command(bf); + return STAT_OK; } stat_t mp_runtime_command(mpBuf_t *bf) { - bf->cm_func(bf->value_vector, bf->flag_vector); // 2 vectors used by callbacks - if (mp_free_run_buffer()) - cm_cycle_end(); // free buffer & perform cycle_end if planner is empty - return (STAT_OK); + bf->cm_func(bf->value_vector, bf->flag_vector); // 2 vectors used by callbacks + if (mp_free_run_buffer()) + cm_cycle_end(); // free buffer & perform cycle_end if planner is empty + return STAT_OK; } /************************************************************************* - * mp_dwell() - queue a dwell + * mp_dwell() - queue a dwell * _exec_dwell() - dwell execution * * Dwells are performed by passing a dwell move to the stepper drivers. @@ -234,23 +234,23 @@ stat_t mp_runtime_command(mpBuf_t *bf) */ stat_t mp_dwell(float seconds) { - mpBuf_t *bf; + mpBuf_t *bf; - if ((bf = mp_get_write_buffer()) == NULL) // get write buffer or fail - return(cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // not ever supposed to fail + if ((bf = mp_get_write_buffer()) == 0) // get write buffer or fail + return(cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // not ever supposed to fail - bf->bf_func = _exec_dwell; // register callback to dwell start - bf->gm.move_time = seconds; // in seconds, not minutes - bf->move_state = MOVE_NEW; - mp_commit_write_buffer(MOVE_TYPE_DWELL); // must be final operation before exit - return (STAT_OK); + bf->bf_func = _exec_dwell; // register callback to dwell start + bf->gm.move_time = seconds; // in seconds, not minutes + bf->move_state = MOVE_NEW; + mp_commit_write_buffer(MOVE_TYPE_DWELL); // must be final operation before exit + return STAT_OK; } static stat_t _exec_dwell(mpBuf_t *bf) { - st_prep_dwell((uint32_t)(bf->gm.move_time * 1000000));// convert seconds to uSec - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & perform cycle_end if planner is empty - return (STAT_OK); + st_prep_dwell((uint32_t)(bf->gm.move_time * 1000000));// convert seconds to uSec + if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & perform cycle_end if planner is empty + return STAT_OK; } /**** PLANNER BUFFERS ***************************************************** @@ -270,172 +270,172 @@ static stat_t _exec_dwell(mpBuf_t *bf) * the command is complete the run buffer is returned to the pool by freeing it. * * Notes: - * The write buffer pointer only moves forward on _queue_write_buffer, and - * the read buffer pointer only moves forward on free_read calls. - * (test, get and unget have no effect) + * The write buffer pointer only moves forward on _queue_write_buffer, and + * the read buffer pointer only moves forward on free_read calls. + * (test, get and unget have no effect) * * mp_get_planner_buffers_available() Returns # of available planner buffers * - * mp_init_buffers() Initializes or resets buffers + * mp_init_buffers() Initializes or resets buffers * - * mp_get_write_buffer() Get pointer to next available write buffer - * Returns pointer or NULL if no buffer available. + * mp_get_write_buffer() Get pointer to next available write buffer + * Returns pointer or 0 if no buffer available. * - * mp_unget_write_buffer() Free write buffer if you decide not to commit it. + * mp_unget_write_buffer() Free write buffer if you decide not to commit it. * - * mp_commit_write_buffer() Commit the next write buffer to the queue - * Advances write pointer & changes buffer state - * WARNING: The calling routine must not use the write buffer - * once it has been queued as it may be processed and freed (wiped) - * before mp_queue_write_buffer() returns. + * mp_commit_write_buffer() Commit the next write buffer to the queue + * Advances write pointer & changes buffer state + * WARNING: The calling routine must not use the write buffer + * once it has been queued as it may be processed and freed (wiped) + * before mp_queue_write_buffer() returns. * - * mp_get_run_buffer() Get pointer to the next or current run buffer - * Returns a new run buffer if prev buf was ENDed - * Returns same buf if called again before ENDing - * Returns NULL if no buffer available - * The behavior supports continuations (iteration) + * mp_get_run_buffer() Get pointer to the next or current run buffer + * Returns a new run buffer if prev buf was ENDed + * Returns same buf if called again before ENDing + * Returns 0 if no buffer available + * The behavior supports continuations (iteration) * - * mp_free_run_buffer() Release the run buffer & return to buffer pool. - * Returns true if queue is empty, false otherwise. - * This is useful for doing queue empty / end move functions. + * mp_free_run_buffer() Release the run buffer & return to buffer pool. + * Returns true if queue is empty, false otherwise. + * This is useful for doing queue empty / end move functions. * - * mp_get_prev_buffer(bf) Returns pointer to prev buffer in linked list - * mp_get_next_buffer(bf) Returns pointer to next buffer in linked list - * mp_get_first_buffer(bf) Returns pointer to first buffer, i.e. the running block - * mp_get_last_buffer(bf) Returns pointer to last buffer, i.e. last block (zero) - * mp_clear_buffer(bf) Zeroes the contents of the buffer - * mp_copy_buffer(bf,bp) Copies the contents of bp into bf - preserves links + * mp_get_prev_buffer(bf) Returns pointer to prev buffer in linked list + * mp_get_next_buffer(bf) Returns pointer to next buffer in linked list + * mp_get_first_buffer(bf) Returns pointer to first buffer, i.e. the running block + * mp_get_last_buffer(bf) Returns pointer to last buffer, i.e. last block (zero) + * mp_clear_buffer(bf) Zeroes the contents of the buffer + * mp_copy_buffer(bf,bp) Copies the contents of bp into bf - preserves links */ -uint8_t mp_get_planner_buffers_available(void) { return (mb.buffers_available);} +uint8_t mp_get_planner_buffers_available() { return mb.buffers_available;} -void mp_init_buffers(void) +void mp_init_buffers() { - mpBuf_t *pv; - uint8_t i; - - memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status - mb.magic_start = MAGICNUM; - mb.magic_end = MAGICNUM; - - mb.w = &mb.bf[0]; // init write and read buffer pointers - mb.q = &mb.bf[0]; - mb.r = &mb.bf[0]; - pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE-1]; - for (i=0; i < PLANNER_BUFFER_POOL_SIZE; i++) { // setup ring pointers - mb.bf[i].nx = &mb.bf[_bump(i)]; - mb.bf[i].pv = pv; - pv = &mb.bf[i]; - } - mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; + mpBuf_t *pv; + uint8_t i; + + memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status + mb.magic_start = MAGICNUM; + mb.magic_end = MAGICNUM; + + mb.w = &mb.bf[0]; // init write and read buffer pointers + mb.q = &mb.bf[0]; + mb.r = &mb.bf[0]; + pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE-1]; + for (i=0; i < PLANNER_BUFFER_POOL_SIZE; i++) { // setup ring pointers + mb.bf[i].nx = &mb.bf[_bump(i)]; + mb.bf[i].pv = pv; + pv = &mb.bf[i]; + } + mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; } -mpBuf_t * mp_get_write_buffer() // get & clear a buffer +mpBuf_t * mp_get_write_buffer() // get & clear a buffer { - if (mb.w->buffer_state == MP_BUFFER_EMPTY) { - mpBuf_t *w = mb.w; - mpBuf_t *nx = mb.w->nx; // save linked list pointers - mpBuf_t *pv = mb.w->pv; - memset(mb.w, 0, sizeof(mpBuf_t)); // clear all values - w->nx = nx; // restore pointers - w->pv = pv; - w->buffer_state = MP_BUFFER_LOADING; - mb.buffers_available--; - mb.w = w->nx; - return (w); - } - rpt_exception(STAT_FAILED_TO_GET_PLANNER_BUFFER); - return (NULL); + if (mb.w->buffer_state == MP_BUFFER_EMPTY) { + mpBuf_t *w = mb.w; + mpBuf_t *nx = mb.w->nx; // save linked list pointers + mpBuf_t *pv = mb.w->pv; + memset(mb.w, 0, sizeof(mpBuf_t)); // clear all values + w->nx = nx; // restore pointers + w->pv = pv; + w->buffer_state = MP_BUFFER_LOADING; + mb.buffers_available--; + mb.w = w->nx; + return w; + } + rpt_exception(STAT_FAILED_TO_GET_PLANNER_BUFFER); + return 0; } void mp_unget_write_buffer() { - mb.w = mb.w->pv; // queued --> write - mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore - mb.buffers_available++; + mb.w = mb.w->pv; // queued --> write + mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore + mb.buffers_available++; } /*** WARNING: The routine calling mp_commit_write_buffer() must not use the write buffer - once it has been queued. Action may start on the buffer immediately, - invalidating its contents ***/ + once it has been queued. Action may start on the buffer immediately, + invalidating its contents ***/ void mp_commit_write_buffer(const uint8_t move_type) { - mb.q->move_type = move_type; - mb.q->move_state = MOVE_NEW; - mb.q->buffer_state = MP_BUFFER_QUEUED; - mb.q = mb.q->nx; // advance the queued buffer pointer - qr_request_queue_report(+1); // request a QR and add to the "added buffers" count - st_request_exec_move(); // requests an exec if the runtime is not busy - // NB: BEWARE! the exec may result in the planner buffer being - // processed immediately and then freed - invalidating the contents + mb.q->move_type = move_type; + mb.q->move_state = MOVE_NEW; + mb.q->buffer_state = MP_BUFFER_QUEUED; + mb.q = mb.q->nx; // advance the queued buffer pointer + qr_request_queue_report(+1); // request a QR and add to the "added buffers" count + st_request_exec_move(); // requests an exec if the runtime is not busy + // NB: BEWARE! the exec may result in the planner buffer being + // processed immediately and then freed - invalidating the contents } 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)) { - mb.r->buffer_state = MP_BUFFER_RUNNING; - } - // CASE: asking for the same run buffer for the Nth time - if (mb.r->buffer_state == MP_BUFFER_RUNNING) { // return same buffer - return (mb.r); - } - return (NULL); // CASE: no queued buffers. fail it. + // CASE: fresh buffer; becomes running if queued or 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 + if (mb.r->buffer_state == MP_BUFFER_RUNNING) { // return same buffer + return mb.r; + } + return 0; // CASE: no queued buffers. fail it. } -uint8_t mp_free_run_buffer() // EMPTY current run buf & adv to next +uint8_t mp_free_run_buffer() // EMPTY current run buf & adv to next { - mp_clear_buffer(mb.r); // clear it out (& reset replannable) -// mb.r->buffer_state = MP_BUFFER_EMPTY; // redundant after the clear, above - mb.r = mb.r->nx; // advance to next run buffer - if (mb.r->buffer_state == MP_BUFFER_QUEUED) {// only if queued... - mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer - } - mb.buffers_available++; - qr_request_queue_report(-1); // request a QR and add to the "removed buffers" count - return ((mb.w == mb.r) ? true : false); // return true if the queue emptied + mp_clear_buffer(mb.r); // clear it out (& reset replannable) +// mb.r->buffer_state = MP_BUFFER_EMPTY; // redundant after the clear, above + mb.r = mb.r->nx; // advance to next run buffer + if (mb.r->buffer_state == MP_BUFFER_QUEUED) {// only if queued... + mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer + } + mb.buffers_available++; + qr_request_queue_report(-1); // request a QR and add to the "removed buffers" count + return (mb.w == mb.r ? true : false); // return true if the queue emptied } -mpBuf_t * mp_get_first_buffer(void) +mpBuf_t * mp_get_first_buffer() { - return(mp_get_run_buffer()); // returns buffer or NULL if nothing's running + return(mp_get_run_buffer()); // returns buffer or 0 if nothing's running } -mpBuf_t * mp_get_last_buffer(void) +mpBuf_t * mp_get_last_buffer() { - mpBuf_t *bf = mp_get_run_buffer(); - mpBuf_t *bp = bf; + mpBuf_t *bf = mp_get_run_buffer(); + mpBuf_t *bp = bf; - if (bf == NULL) return(NULL); + if (bf == 0) return(0); - do { - if ((bp->nx->move_state == MOVE_OFF) || (bp->nx == bf)) { - return (bp); - } - } while ((bp = mp_get_next_buffer(bp)) != bf); - return (bp); + do { + if ((bp->nx->move_state == MOVE_OFF) || (bp->nx == bf)) { + return bp; + } + } while ((bp = mp_get_next_buffer(bp)) != bf); + return bp; } // Use the macro instead -//mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf) return (bf->pv); -//mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf) return (bf->nx); +//mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf) return bf->pv; +//mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf) return bf->nx; void mp_clear_buffer(mpBuf_t *bf) { - mpBuf_t *nx = bf->nx; // save pointers - mpBuf_t *pv = bf->pv; - memset(bf, 0, sizeof(mpBuf_t)); - bf->nx = nx; // restore pointers - bf->pv = pv; + mpBuf_t *nx = bf->nx; // save pointers + mpBuf_t *pv = bf->pv; + memset(bf, 0, sizeof(mpBuf_t)); + bf->nx = nx; // restore pointers + bf->pv = pv; } void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp) { - mpBuf_t *nx = bf->nx; // save pointers - mpBuf_t *pv = bf->pv; - memcpy(bf, bp, sizeof(mpBuf_t)); - bf->nx = nx; // restore pointers - bf->pv = pv; + mpBuf_t *nx = bf->nx; // save pointers + mpBuf_t *pv = bf->pv; + memcpy(bf, bp, sizeof(mpBuf_t)); + bf->nx = nx; // restore pointers + bf->pv = pv; } diff --git a/src/planner.h b/src/planner.h index 3dbcfa0..b6dbf85 100755 --- a/src/planner.h +++ b/src/planner.h @@ -29,286 +29,286 @@ #ifndef PLANNER_H_ONCE #define PLANNER_H_ONCE -#include "canonical_machine.h" // used for GCodeState_t - -enum moveType { // bf->move_type values - MOVE_TYPE_NULL = 0, // null move - does a no-op - MOVE_TYPE_ALINE, // acceleration planned line - MOVE_TYPE_DWELL, // delay with no movement - MOVE_TYPE_COMMAND, // general command - MOVE_TYPE_TOOL, // T command - MOVE_TYPE_SPINDLE_SPEED,// S command - MOVE_TYPE_STOP, // program stop - MOVE_TYPE_END // program end +#include "canonical_machine.h" // used for GCodeState_t + +enum moveType { // bf->move_type values + MOVE_TYPE_0 = 0, // null move - does a no-op + MOVE_TYPE_ALINE, // acceleration planned line + MOVE_TYPE_DWELL, // delay with no movement + MOVE_TYPE_COMMAND, // general command + MOVE_TYPE_TOOL, // T command + MOVE_TYPE_SPINDLE_SPEED,// S command + MOVE_TYPE_STOP, // program stop + MOVE_TYPE_END // program end }; enum moveState { - MOVE_OFF = 0, // move inactive (MUST BE ZERO) - MOVE_NEW, // general value if you need an initialization - MOVE_RUN, // general run state (for non-acceleration moves) - MOVE_SKIP_BLOCK // mark a skipped block + MOVE_OFF = 0, // move inactive (MUST BE ZERO) + MOVE_NEW, // general value if you need an initialization + MOVE_RUN, // general run state (for non-acceleration moves) + MOVE_SKIP_BLOCK // mark a skipped block }; enum moveSection { - SECTION_HEAD = 0, // acceleration - SECTION_BODY, // cruise - SECTION_TAIL // deceleration + SECTION_HEAD = 0, // acceleration + SECTION_BODY, // cruise + SECTION_TAIL // deceleration }; #define SECTIONS 3 enum sectionState { - SECTION_OFF = 0, // section inactive - SECTION_NEW, // uninitialized section - SECTION_1st_HALF, // first half of S curve - SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) + SECTION_OFF = 0, // section inactive + SECTION_NEW, // uninitialized section + SECTION_1st_HALF, // first half of S curve + SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) }; /*** Most of these factors are the result of a lot of tweaking. Change with caution.***/ -#define ARC_SEGMENT_LENGTH ((float)0.1) // Arc segment size (mm).(0.03) +#define ARC_SEGMENT_LENGTH ((float)0.1) // Arc segment size (mm).(0.03) #define MIN_ARC_RADIUS ((float)0.1) #define JERK_MULTIPLIER ((float)1000000) -#define JERK_MATCH_PRECISION ((float)1000) // precision to which jerk must match to be considered effectively the same +#define JERK_MATCH_PRECISION ((float)1000) // precision to which jerk must match to be considered effectively the same -#define NOM_SEGMENT_USEC ((float)5000) // nominal segment time -#define MIN_SEGMENT_USEC ((float)2500) // minimum segment time / minimum move time -#define MIN_ARC_SEGMENT_USEC ((float)10000) // minimum arc segment time +#define NOM_SEGMENT_USEC ((float)5000) // nominal segment time +#define MIN_SEGMENT_USEC ((float)2500) // minimum segment time / minimum move time +#define MIN_ARC_SEGMENT_USEC ((float)10000) // minimum arc segment time #define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) #define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) #define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_TIME_MOVE MIN_SEGMENT_TIME // minimum time a move can be is one segment -#define MIN_BLOCK_TIME MIN_SEGMENT_TIME // factor for minimum size Gcode block to process +#define MIN_TIME_MOVE MIN_SEGMENT_TIME // minimum time a move can be is one segment +#define MIN_BLOCK_TIME MIN_SEGMENT_TIME // factor for minimum size Gcode block to process #define MIN_SEGMENT_TIME_PLUS_MARGIN ((MIN_SEGMENT_USEC+1) / MICROSECONDS_PER_MINUTE) /* PLANNER_STARTUP_DELAY_SECONDS - * Used to introduce a short dwell before planning an idle machine. + * Used to introduce a short dwell before planning an idle machine. * If you don't do this the first block will always plan to zero as it will - * start executing before the next block arrives from the serial port. - * This causes the machine to stutter once on startup. + * start executing before the next block arrives from the serial port. + * This causes the machine to stutter once on startup. */ -//#define PLANNER_STARTUP_DELAY_SECONDS ((float)0.05) // in seconds +//#define PLANNER_STARTUP_DELAY_SECONDS ((float)0.05) // in seconds /* PLANNER_BUFFER_POOL_SIZE - * Should be at least the number of buffers requires to support optimal - * planning in the case of very short lines or arc segments. - * Suggest 12 min. Limit is 255 + * Should be at least the number of buffers requires to support optimal + * planning in the case of very short lines or arc segments. + * Suggest 12 min. Limit is 255 */ #define PLANNER_BUFFER_POOL_SIZE 32 -#define PLANNER_BUFFER_HEADROOM 4 // buffers to reserve in planner before processing new input line +#define PLANNER_BUFFER_HEADROOM 4 // buffers to reserve in planner before processing new input line /* Some parameters for _generate_trapezoid() - * TRAPEZOID_ITERATION_MAX Max iterations for convergence in the HT asymmetric case. - * TRAPEZOID_ITERATION_ERROR_PERCENT Error percentage for iteration convergence. As percent - 0.01 = 1% - * TRAPEZOID_LENGTH_FIT_TOLERANCE Tolerance for "exact fit" for H and T cases - * TRAPEZOID_VELOCITY_TOLERANCE Adaptive velocity tolerance term + * TRAPEZOID_ITERATION_MAX Max iterations for convergence in the HT asymmetric case. + * TRAPEZOID_ITERATION_ERROR_PERCENT Error percentage for iteration convergence. As percent - 0.01 = 1% + * TRAPEZOID_LENGTH_FIT_TOLERANCE Tolerance for "exact fit" for H and T cases + * TRAPEZOID_VELOCITY_TOLERANCE Adaptive velocity tolerance term */ -#define TRAPEZOID_ITERATION_MAX 10 -#define TRAPEZOID_ITERATION_ERROR_PERCENT ((float)0.10) -#define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) // allowable mm of error in planning phase -#define TRAPEZOID_VELOCITY_TOLERANCE (max(2,bf->entry_velocity/100)) +#define TRAPEZOID_ITERATION_MAX 10 +#define TRAPEZOID_ITERATION_ERROR_PERCENT ((float)0.10) +#define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) // allowable mm of error in planning phase +#define TRAPEZOID_VELOCITY_TOLERANCE (max(2,bf->entry_velocity/100)) /* - * Macros and typedefs + * Macros and typedefs */ -typedef void (*cm_exec_t)(float[], float[]); // callback to canonical_machine execution function +typedef void (*cm_exec_t)(float[], float[]); // callback to canonical_machine execution function /* - * Planner structures + * Planner structures */ // All the enums that equal zero must be zero. Don't change this -enum mpBufferState { // bf->buffer_state values - MP_BUFFER_EMPTY = 0, // struct is available for use (MUST BE 0) - MP_BUFFER_LOADING, // being written ("checked out") - MP_BUFFER_QUEUED, // in queue - MP_BUFFER_PENDING, // marked as the next buffer to run - MP_BUFFER_RUNNING // current running buffer +enum mpBufferState { // bf->buffer_state values + MP_BUFFER_EMPTY = 0, // struct is available for use (MUST BE 0) + MP_BUFFER_LOADING, // being written ("checked out") + MP_BUFFER_QUEUED, // in queue + MP_BUFFER_PENDING, // marked as the next buffer to run + MP_BUFFER_RUNNING // current running buffer }; -typedef struct mpBuffer { // See Planning Velocity Notes for variable usage - struct mpBuffer *pv; // static pointer to previous buffer - struct mpBuffer *nx; // static pointer to next buffer - stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function - cm_exec_t cm_func; // callback to canonical machine execution function +typedef struct mpBuffer { // See Planning Velocity Notes for variable usage + struct mpBuffer *pv; // static pointer to previous buffer + struct mpBuffer *nx; // static pointer to next buffer + stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function + cm_exec_t cm_func; // callback to canonical machine execution function - float naiive_move_time; + float naiive_move_time; - uint8_t buffer_state; // used to manage queuing/dequeuing - uint8_t move_type; // used to dispatch to run routine - uint8_t move_code; // byte that can be used by used exec functions - uint8_t move_state; // move state machine sequence - uint8_t replannable; // TRUE if move can be re-planned + uint8_t buffer_state; // used to manage queuing/dequeuing + uint8_t move_type; // used to dispatch to run routine + uint8_t move_code; // byte that can be used by used exec functions + uint8_t move_state; // move state machine sequence + uint8_t replannable; // TRUE if move can be re-planned - float unit[AXES]; // unit vector for axis scaling & planning + float unit[AXES]; // unit vector for axis scaling & planning - float length; // total length of line or helix in mm - float head_length; - float body_length; - float tail_length; - // *** SEE NOTES ON THESE VARIABLES, in aline() *** - float entry_velocity; // entry velocity requested for the move - float cruise_velocity; // cruise velocity requested & achieved - float exit_velocity; // exit velocity requested for the move + float length; // total length of line or helix in mm + float head_length; + float body_length; + float tail_length; + // *** SEE NOTES ON THESE VARIABLES, in aline() *** + float entry_velocity; // entry velocity requested for the move + float cruise_velocity; // cruise velocity requested & achieved + float exit_velocity; // exit velocity requested for the move - float entry_vmax; // max junction velocity at entry of this move - float cruise_vmax; // max cruise velocity requested for move - float exit_vmax; // max exit velocity possible (redundant) - float delta_vmax; // max velocity difference for this move - float braking_velocity; // current value for braking velocity + float entry_vmax; // max junction velocity at entry of this move + float cruise_vmax; // max cruise velocity requested for move + float exit_vmax; // max exit velocity possible (redundant) + float delta_vmax; // max velocity difference for this move + float braking_velocity; // current value for braking velocity - uint8_t jerk_axis; // rate limiting axis used to compute jerk for the move - float jerk; // maximum linear jerk term for this move - float recip_jerk; // 1/Jm used for planning (computed and cached) - float cbrt_jerk; // cube root of Jm used for planning (computed and cached) + uint8_t jerk_axis; // rate limiting axis used to compute jerk for the move + float jerk; // maximum linear jerk term for this move + float recip_jerk; // 1/Jm used for planning (computed and cached) + float cbrt_jerk; // cube root of Jm used for planning (computed and cached) - GCodeState_t gm; // Gode model state - passed from model, used by planner and runtime + GCodeState_t gm; // Gode model state - passed from model, used by planner and runtime } mpBuf_t; -typedef struct mpBufferPool { // ring buffer for sub-moves - magic_t magic_start; // magic number to test memory integrity - uint8_t buffers_available; // running count of available buffers - mpBuf_t *w; // get_write_buffer pointer - mpBuf_t *q; // queue_write_buffer pointer - mpBuf_t *r; // get/end_run_buffer pointer - mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE];// buffer storage - magic_t magic_end; +typedef struct mpBufferPool { // ring buffer for sub-moves + magic_t magic_start; // magic number to test memory integrity + uint8_t buffers_available; // running count of available buffers + mpBuf_t *w; // get_write_buffer pointer + mpBuf_t *q; // queue_write_buffer pointer + mpBuf_t *r; // get/end_run_buffer pointer + mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE];// buffer storage + magic_t magic_end; } mpBufferPool_t; typedef struct mpMoveMasterSingleton { // common variables for planning (move master) - magic_t magic_start; // magic number to test memory integrity - float position[AXES]; // final move position for planning purposes + magic_t magic_start; // magic number to test memory integrity + float position[AXES]; // final move position for planning purposes - float jerk; // jerk values cached from previous block - float recip_jerk; - float cbrt_jerk; + float jerk; // jerk values cached from previous block + float recip_jerk; + float cbrt_jerk; - magic_t magic_end; + magic_t magic_end; } mpMoveMasterSingleton_t; -typedef struct mpMoveRuntimeSingleton { // persistent runtime variables -// uint8_t (*run_move)(struct mpMoveRuntimeSingleton *m); // currently running move - left in for reference - magic_t magic_start; // magic number to test memory integrity - uint8_t move_state; // state of the overall move - uint8_t section; // what section is the move in? - uint8_t section_state; // state within a move section - - float unit[AXES]; // unit vector for axis scaling & planning - float target[AXES]; // final target for bf (used to correct rounding errors) - float position[AXES]; // current move position - float position_c[AXES]; // for Kahan summation in _exec_aline_segment() - float waypoint[SECTIONS][AXES]; // head/body/tail endpoints for correction - - float target_steps[MOTORS]; // current MR target (absolute target as steps) - float position_steps[MOTORS]; // current MR position (target from previous segment) - float commanded_steps[MOTORS]; // will align with next encoder sample (target from 2nd previous segment) - float encoder_steps[MOTORS]; // encoder position in steps - ideally the same as commanded_steps - float following_error[MOTORS]; // difference between encoder_steps and commanded steps - - float head_length; // copies of bf variables of same name - float body_length; - float tail_length; - - float entry_velocity; - float cruise_velocity; - float exit_velocity; - - float segments; // number of segments in line (also used by arc generation) - uint32_t segment_count; // count of running segments - float segment_velocity; // computed velocity for aline segment - float segment_time; // actual time increment per aline segment - float jerk; // max linear jerk - -#ifdef __JERK_EXEC // values used exclusively by computed jerk acceleration - float jerk_div2; // cached value for efficiency - float midpoint_velocity; // velocity at accel/decel midpoint - float midpoint_acceleration; // - float accel_time; // - float segment_accel_time; // - float elapsed_accel_time; // -#else // values used exclusively by forward differencing acceleration - float forward_diff_1; // forward difference level 1 - float forward_diff_2; // forward difference level 2 - float forward_diff_3; // forward difference level 3 - float forward_diff_4; // forward difference level 4 - float forward_diff_5; // forward difference level 5 +typedef struct mpMoveRuntimeSingleton { // persistent runtime variables +// uint8_t (*run_move)(struct mpMoveRuntimeSingleton *m); // currently running move - left in for reference + magic_t magic_start; // magic number to test memory integrity + uint8_t move_state; // state of the overall move + uint8_t section; // what section is the move in? + uint8_t section_state; // state within a move section + + float unit[AXES]; // unit vector for axis scaling & planning + float target[AXES]; // final target for bf (used to correct rounding errors) + float position[AXES]; // current move position + float position_c[AXES]; // for Kahan summation in _exec_aline_segment() + float waypoint[SECTIONS][AXES]; // head/body/tail endpoints for correction + + float target_steps[MOTORS]; // current MR target (absolute target as steps) + float position_steps[MOTORS]; // current MR position (target from previous segment) + float commanded_steps[MOTORS]; // will align with next encoder sample (target from 2nd previous segment) + float encoder_steps[MOTORS]; // encoder position in steps - ideally the same as commanded_steps + float following_error[MOTORS]; // difference between encoder_steps and commanded steps + + float head_length; // copies of bf variables of same name + float body_length; + float tail_length; + + float entry_velocity; + float cruise_velocity; + float exit_velocity; + + float segments; // number of segments in line (also used by arc generation) + uint32_t segment_count; // count of running segments + float segment_velocity; // computed velocity for aline segment + float segment_time; // actual time increment per aline segment + float jerk; // max linear jerk + +#ifdef __JERK_EXEC // values used exclusively by computed jerk acceleration + float jerk_div2; // cached value for efficiency + float midpoint_velocity; // velocity at accel/decel midpoint + float midpoint_acceleration; // + float accel_time; // + float segment_accel_time; // + float elapsed_accel_time; // +#else // values used exclusively by forward differencing acceleration + float forward_diff_1; // forward difference level 1 + float forward_diff_2; // forward difference level 2 + float forward_diff_3; // forward difference level 3 + float forward_diff_4; // forward difference level 4 + float forward_diff_5; // forward difference level 5 #ifdef __KAHAN - float forward_diff_1_c; // forward difference level 1 floating-point compensation - float forward_diff_2_c; // forward difference level 2 floating-point compensation - float forward_diff_3_c; // forward difference level 3 floating-point compensation - float forward_diff_4_c; // forward difference level 4 floating-point compensation - float forward_diff_5_c; // forward difference level 5 floating-point compensation + float forward_diff_1_c; // forward difference level 1 floating-point compensation + float forward_diff_2_c; // forward difference level 2 floating-point compensation + float forward_diff_3_c; // forward difference level 3 floating-point compensation + float forward_diff_4_c; // forward difference level 4 floating-point compensation + float forward_diff_5_c; // forward difference level 5 floating-point compensation #endif #endif - GCodeState_t gm; // gcode model state currently executing + GCodeState_t gm; // gcode model state currently executing - magic_t magic_end; + magic_t magic_end; } mpMoveRuntimeSingleton_t; // Reference global scope structures -extern mpBufferPool_t mb; // move buffer queue -extern mpMoveMasterSingleton_t mm; // context for line planning -extern mpMoveRuntimeSingleton_t mr; // context for line runtime +extern mpBufferPool_t mb; // move buffer queue +extern mpMoveMasterSingleton_t mm; // context for line planning +extern mpMoveRuntimeSingleton_t mr; // context for line runtime /* * Global Scope Functions */ -void planner_init(void); -void planner_init_assertions(void); -stat_t planner_test_assertions(void); +void planner_init(); +void planner_init_assertions(); +stat_t planner_test_assertions(); -void mp_flush_planner(void); +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(void); +void mp_set_steps_to_runtime_position(); void mp_queue_command(void(*cm_exec_t)(float[], float[]), float *value, float *flag); stat_t mp_runtime_command(mpBuf_t *bf); stat_t mp_dwell(const float seconds); -void mp_end_dwell(void); +void mp_end_dwell(); stat_t mp_aline(GCodeState_t *gm_in); -stat_t mp_plan_hold_callback(void); -stat_t mp_end_hold(void); +stat_t mp_plan_hold_callback(); +stat_t mp_end_hold(); stat_t mp_feed_rate_override(uint8_t flag, float parameter); // planner buffer handlers -uint8_t mp_get_planner_buffers_available(void); -void mp_init_buffers(void); -mpBuf_t * mp_get_write_buffer(void); -void mp_unget_write_buffer(void); +uint8_t mp_get_planner_buffers_available(); +void mp_init_buffers(); +mpBuf_t * mp_get_write_buffer(); +void mp_unget_write_buffer(); void mp_commit_write_buffer(const uint8_t move_type); -mpBuf_t * mp_get_run_buffer(void); -uint8_t mp_free_run_buffer(void); -mpBuf_t * mp_get_first_buffer(void); -mpBuf_t * mp_get_last_buffer(void); +mpBuf_t * mp_get_run_buffer(); +uint8_t mp_free_run_buffer(); +mpBuf_t * mp_get_first_buffer(); +mpBuf_t * mp_get_last_buffer(); //mpBuf_t * mp_get_prev_buffer(const mpBuf_t *bf); //mpBuf_t * mp_get_next_buffer(const mpBuf_t *bf); -#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) // use the macro instead +#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) // use the macro instead #define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) void mp_clear_buffer(mpBuf_t *bf); void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); // plan_line.c functions -float mp_get_runtime_velocity(void); +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(void); -uint8_t mp_get_runtime_busy(void); -float* mp_get_planner_position_vector(void); +void mp_zero_segment_velocity(); +uint8_t mp_get_runtime_busy(); +float* mp_get_planner_position_vector(); // plan_zoid.c functions void mp_calculate_trapezoid(mpBuf_t *bf); @@ -316,7 +316,7 @@ float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); // plan_exec.c functions -stat_t mp_exec_move(void); +stat_t mp_exec_move(); stat_t mp_exec_aline(mpBuf_t *bf); -#endif // End of include Guard: PLANNER_H_ONCE +#endif // End of include Guard: PLANNER_H_ONCE diff --git a/src/pwm.c b/src/pwm.c index d0c4498..828839c 100755 --- a/src/pwm.c +++ b/src/pwm.c @@ -25,8 +25,8 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "hardware.h" #include "text_parser.h" #include "gpio.h" @@ -39,61 +39,61 @@ pwmSingleton_t pwm; // defines common to all PWM channels -//#define PWM_TIMER_TYPE TC1_struct // PWM uses TC1's -#define PWM_TIMER_t TC1_t // PWM uses TC1's -#define PWM_TIMER_DISABLE 0 // turn timer off (clock = 0 Hz) -#define PWM_MAX_FREQ (F_CPU/256) // max frequency with 8-bits duty cycle precision -#define PWM_MIN_FREQ (F_CPU/64/65536) // min frequency with supported prescaling +//#define PWM_TIMER_TYPE TC1_struct // PWM uses TC1's +#define PWM_TIMER_t TC1_t // PWM uses TC1's +#define PWM_TIMER_DISABLE 0 // turn timer off (clock = 0 Hz) +#define PWM_MAX_FREQ (F_CPU/256) // max frequency with 8-bits duty cycle precision +#define PWM_MIN_FREQ (F_CPU/64/65536) // min frequency with supported prescaling // channel specific defines /* CLKSEL is used to configure default PWM clock operating ranges * These can be changed by pwm_freq() depending on the PWM frequency selected * * The useful ranges (assuming a 32 Mhz system clock) are: - * TC_CLKSEL_DIV1_gc - good for about 500 Hz to 125 Khz practical upper limit + * TC_CLKSEL_DIV1_gc - good for about 500 Hz to 125 Khz practical upper limit * TC_CLKSEL_DIV2_gc - good for about 250 Hz to 62 KHz - * TC_CLKSEL_DIV4_gc - good for about 125 Hz to 31 KHz - * TC_CLKSEL_DIV8_gc - good for about 62 Hz to 16 KHz - * TC_CLKSEL_DIV64_gc - good for about 8 Hz to 2 Khz + * TC_CLKSEL_DIV4_gc - good for about 125 Hz to 31 KHz + * TC_CLKSEL_DIV8_gc - good for about 62 Hz to 16 KHz + * TC_CLKSEL_DIV64_gc - good for about 8 Hz to 2 Khz */ -#define PWM1_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc // starting clock select value -#define PWM1_CTRLB (3 | TC0_CCBEN_bm) // single slope PWM enabled on channel B -#define PWM1_ISR_vect TCD1_CCB_vect // must match timer assignments in system.h -#define PWM1_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) +#define PWM1_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc // starting clock select value +#define PWM1_CTRLB (3 | TC0_CCBEN_bm) // single slope PWM enabled on channel B +#define PWM1_ISR_vect TCD1_CCB_vect // must match timer assignments in system.h +#define PWM1_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) -#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc -#define PWM2_CTRLB 3 // single slope PWM enabled, no output channel -//#define PWM2_CTRLB (3 | TC0_CCBEN_bm) // single slope PWM enabled on channel B -#define PWM2_ISR_vect TCE1_CCB_vect // must match timer assignments in system.h -#define PWM2_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) +#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc +#define PWM2_CTRLB 3 // single slope PWM enabled, no output channel +//#define PWM2_CTRLB (3 | TC0_CCBEN_bm) // single slope PWM enabled on channel B +#define PWM2_ISR_vect TCE1_CCB_vect // must match timer assignments in system.h +#define PWM2_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) /***** PWM code *****/ /* * pwm_init() - initialize pwm channels * - * Notes: - * - Whatever level interrupts you use must be enabled in main() - * - init assumes PWM1 output bit (D5) has been set to output previously (stepper.c) - * - See system.h for timer and port assignments + * Notes: + * - Whatever level interrupts you use must be enabled in main() + * - init assumes PWM1 output bit (D5) has been set to output previously (stepper.c) + * - See system.h for timer and port assignments * - Don't do this: memset(&TIMER_PWM1, 0, sizeof(PWM_TIMER_t)); // zero out the timer registers */ void pwm_init() { gpio_set_bit_off(SPINDLE_PWM); - // setup PWM channel 1 - memset(&pwm.p[PWM_1], 0, sizeof(pwmChannel_t)); // clear parent structure - pwm.p[PWM_1].timer = &TIMER_PWM1; // bind timer struct to PWM struct array - pwm.p[PWM_1].ctrla = PWM1_CTRLA_CLKSEL; // initialize starting clock operating range - pwm.p[PWM_1].timer->CTRLB = PWM1_CTRLB; - pwm.p[PWM_1].timer->INTCTRLB = PWM1_INTCTRLB; // set interrupt level - - // setup PWM channel 2 - memset(&pwm.p[PWM_2], 0, sizeof(pwmChannel_t)); // clear all values, pointers and status - pwm.p[PWM_2].timer = &TIMER_PWM2; - pwm.p[PWM_2].ctrla = PWM2_CTRLA_CLKSEL; - pwm.p[PWM_2].timer->CTRLB = PWM2_CTRLB; - pwm.p[PWM_2].timer->INTCTRLB = PWM2_INTCTRLB; + // setup PWM channel 1 + memset(&pwm.p[PWM_1], 0, sizeof(pwmChannel_t)); // clear parent structure + pwm.p[PWM_1].timer = &TIMER_PWM1; // bind timer struct to PWM struct array + pwm.p[PWM_1].ctrla = PWM1_CTRLA_CLKSEL; // initialize starting clock operating range + pwm.p[PWM_1].timer->CTRLB = PWM1_CTRLB; + pwm.p[PWM_1].timer->INTCTRLB = PWM1_INTCTRLB; // set interrupt level + + // setup PWM channel 2 + memset(&pwm.p[PWM_2], 0, sizeof(pwmChannel_t)); // clear all values, pointers and status + pwm.p[PWM_2].timer = &TIMER_PWM2; + pwm.p[PWM_2].ctrla = PWM2_CTRLA_CLKSEL; + pwm.p[PWM_2].timer->CTRLB = PWM2_CTRLB; + pwm.p[PWM_2].timer->INTCTRLB = PWM2_INTCTRLB; } /* @@ -101,76 +101,76 @@ void pwm_init() */ ISR(PWM1_ISR_vect) { - return; + return; } ISR(PWM2_ISR_vect) { - return; + return; } /* * pwm_set_freq() - set PWM channel frequency * - * channel - PWM channel - * freq - PWM frequency in Khz as a float + * channel - PWM channel + * freq - PWM frequency in Khz as a float * - * Assumes 32MHz clock. - * Doesn't turn time on until duty cycle is set + * Assumes 32MHz clock. + * Doesn't turn time on until duty cycle is set */ stat_t pwm_set_freq(uint8_t chan, float freq) { - if (chan > PWMS) { return (STAT_NO_SUCH_DEVICE);} - if (freq > PWM_MAX_FREQ) { return (STAT_INPUT_EXCEEDS_MAX_VALUE);} - if (freq < PWM_MIN_FREQ) { return (STAT_INPUT_LESS_THAN_MIN_VALUE);} + if (chan > PWMS) { return STAT_NO_SUCH_DEVICE;} + if (freq > PWM_MAX_FREQ) { return STAT_INPUT_EXCEEDS_MAX_VALUE;} + if (freq < PWM_MIN_FREQ) { return STAT_INPUT_LESS_THAN_MIN_VALUE;} // set the period and the prescaler - float prescale = F_CPU/65536/freq; // optimal non-integer prescaler value - if (prescale <= 1) { - pwm.p[chan].timer->PER = F_CPU/freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV1_gc; - } else if (prescale <= 2) { - pwm.p[chan].timer->PER = F_CPU/2/freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV2_gc; - } else if (prescale <= 4) { - pwm.p[chan].timer->PER = F_CPU/4/freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV4_gc; - } else if (prescale <= 8) { - pwm.p[chan].timer->PER = F_CPU/8/freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV8_gc; - } else { - pwm.p[chan].timer->PER = F_CPU/64/freq; - pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV64_gc; - } - - return (STAT_OK); + float prescale = F_CPU/65536/freq; // optimal non-integer prescaler value + if (prescale <= 1) { + pwm.p[chan].timer->PER = F_CPU/freq; + pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV1_gc; + } else if (prescale <= 2) { + pwm.p[chan].timer->PER = F_CPU/2/freq; + pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV2_gc; + } else if (prescale <= 4) { + pwm.p[chan].timer->PER = F_CPU/4/freq; + pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV4_gc; + } else if (prescale <= 8) { + pwm.p[chan].timer->PER = F_CPU/8/freq; + pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV8_gc; + } else { + pwm.p[chan].timer->PER = F_CPU/64/freq; + pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV64_gc; + } + + return STAT_OK; } /* * pwm_set_duty() - set PWM channel duty cycle * - * channel - PWM channel - * duty - PWM duty cycle from 0% to 100% + * channel - PWM channel + * duty - PWM duty cycle from 0% to 100% * - * Setting duty cycle to 0 disables the PWM channel with output low - * Setting duty cycle to 100 disables the PWM channel with output high - * Setting duty cycle between 0 and 100 enables PWM channel + * Setting duty cycle to 0 disables the PWM channel with output low + * Setting duty cycle to 100 disables the PWM channel with output high + * Setting duty cycle between 0 and 100 enables PWM channel * - * The frequency must have been set previously + * The frequency must have been set previously */ stat_t pwm_set_duty(uint8_t chan, float duty) { - if (duty < 0.0) { return (STAT_INPUT_LESS_THAN_MIN_VALUE);} - if (duty > 1.0) { return (STAT_INPUT_EXCEEDS_MAX_VALUE);} + if (duty < 0.0) { return STAT_INPUT_LESS_THAN_MIN_VALUE;} + if (duty > 1.0) { return STAT_INPUT_EXCEEDS_MAX_VALUE;} // Ffrq = Fper/(2N(CCA+1)) // Fpwm = Fper/((N(PER+1)) - float period_scalar = pwm.p[chan].timer->PER; - pwm.p[chan].timer->CCB = (uint16_t)(period_scalar * duty) + 1; + float period_scalar = pwm.p[chan].timer->PER; + pwm.p[chan].timer->CCB = (uint16_t)(period_scalar * duty) + 1; - return (STAT_OK); + return STAT_OK; } diff --git a/src/pwm.h b/src/pwm.h index 4723afa..ecc0926 100755 --- a/src/pwm.h +++ b/src/pwm.h @@ -29,62 +29,62 @@ #define PWM_H_ONCE typedef struct pwmConfigChannel { - float frequency; // base frequency for PWM driver, in Hz - float cw_speed_lo; // minimum clockwise spindle speed [0..N] - float cw_speed_hi; // maximum clockwise spindle speed - float cw_phase_lo; // pwm phase at minimum CW spindle speed, clamped [0..1] - float cw_phase_hi; // pwm phase at maximum CW spindle speed, clamped [0..1] - float ccw_speed_lo; // minimum counter-clockwise spindle speed [0..N] - float ccw_speed_hi; // maximum counter-clockwise spindle speed - float ccw_phase_lo; // pwm phase at minimum CCW spindle speed, clamped [0..1] - float ccw_phase_hi; // pwm phase at maximum CCW spindle speed, clamped - float phase_off; // pwm phase when spindle is disabled + float frequency; // base frequency for PWM driver, in Hz + float cw_speed_lo; // minimum clockwise spindle speed [0..N] + float cw_speed_hi; // maximum clockwise spindle speed + float cw_phase_lo; // pwm phase at minimum CW spindle speed, clamped [0..1] + float cw_phase_hi; // pwm phase at maximum CW spindle speed, clamped [0..1] + float ccw_speed_lo; // minimum counter-clockwise spindle speed [0..N] + float ccw_speed_hi; // maximum counter-clockwise spindle speed + float ccw_phase_lo; // pwm phase at minimum CCW spindle speed, clamped [0..1] + float ccw_phase_hi; // pwm phase at maximum CCW spindle speed, clamped + float phase_off; // pwm phase when spindle is disabled } pwmConfigChannel_t; typedef struct pwmChannel { - uint8_t ctrla; // byte needed to active CTRLA (it's dynamic - rest are static) - TC1_t *timer; // assumes TC1 flavor timers used for PWM channels + uint8_t ctrla; // byte needed to active CTRLA (it's dynamic - rest are static) + TC1_t *timer; // assumes TC1 flavor timers used for PWM channels } pwmChannel_t; typedef struct pwmSingleton { - pwmConfigChannel_t c[PWMS]; // array of channel configs - pwmChannel_t p[PWMS]; // array of PWM channels + pwmConfigChannel_t c[PWMS]; // array of channel configs + pwmChannel_t p[PWMS]; // array of PWM channels } pwmSingleton_t; extern pwmSingleton_t pwm; /*** function prototypes ***/ -void pwm_init(void); +void pwm_init(); stat_t pwm_set_freq(uint8_t channel, float freq); stat_t pwm_set_duty(uint8_t channel, float duty); #ifdef __TEXT_MODE - void pwm_print_p1frq(nvObj_t *nv); - void pwm_print_p1csl(nvObj_t *nv); - void pwm_print_p1csh(nvObj_t *nv); - void pwm_print_p1cpl(nvObj_t *nv); - void pwm_print_p1cph(nvObj_t *nv); - void pwm_print_p1wsl(nvObj_t *nv); - void pwm_print_p1wsh(nvObj_t *nv); - void pwm_print_p1wpl(nvObj_t *nv); - void pwm_print_p1wph(nvObj_t *nv); - void pwm_print_p1pof(nvObj_t *nv); + void pwm_print_p1frq(nvObj_t *nv); + void pwm_print_p1csl(nvObj_t *nv); + void pwm_print_p1csh(nvObj_t *nv); + void pwm_print_p1cpl(nvObj_t *nv); + void pwm_print_p1cph(nvObj_t *nv); + void pwm_print_p1wsl(nvObj_t *nv); + void pwm_print_p1wsh(nvObj_t *nv); + void pwm_print_p1wpl(nvObj_t *nv); + void pwm_print_p1wph(nvObj_t *nv); + void pwm_print_p1pof(nvObj_t *nv); #else - #define pwm_print_p1frq tx_print_stub - #define pwm_print_p1csl tx_print_stub - #define pwm_print_p1csh tx_print_stub - #define pwm_print_p1cpl tx_print_stub - #define pwm_print_p1cph tx_print_stub - #define pwm_print_p1wsl tx_print_stub - #define pwm_print_p1wsh tx_print_stub - #define pwm_print_p1wpl tx_print_stub - #define pwm_print_p1wph tx_print_stub - #define pwm_print_p1pof tx_print_stub + #define pwm_print_p1frq tx_print_stub + #define pwm_print_p1csl tx_print_stub + #define pwm_print_p1csh tx_print_stub + #define pwm_print_p1cpl tx_print_stub + #define pwm_print_p1cph tx_print_stub + #define pwm_print_p1wsl tx_print_stub + #define pwm_print_p1wsh tx_print_stub + #define pwm_print_p1wpl tx_print_stub + #define pwm_print_p1wph tx_print_stub + #define pwm_print_p1pof tx_print_stub #endif // __TEXT_MODE -#endif // End of include guard: PWM_H_ONCE +#endif // End of include guard: PWM_H_ONCE diff --git a/src/report.c b/src/report.c index 92d04c8..72b961d 100755 --- a/src/report.c +++ b/src/report.c @@ -34,7 +34,7 @@ #include "planner.h" #include "settings.h" #include "util.h" -#include "xio.h" +#include "xio/xio.h" /**** Allocation ****/ @@ -48,237 +48,237 @@ rxSingleton_t rx; * Returns incoming status value * * WARNING: Do not call this function from MED or HI interrupts (LO is OK) - * or there is a potential for deadlock in the TX buffer. + * or there is a potential for deadlock in the TX buffer. */ stat_t rpt_exception(uint8_t status) { - if (status != STAT_OK) { // makes it possible to call exception reports w/o checking status value - if (js.json_syntax == JSON_SYNTAX_RELAXED) { - printf_P(PSTR("{er:{fb:%0.2f,st:%d,msg:\"%s\"}}\n"), - TINYG_FIRMWARE_BUILD, status, get_status_message(status)); - } else { - printf_P(PSTR("{\"er\":{\"fb\":%0.2f,\"st\":%d,\"msg\":\"%s\"}}\n"), - TINYG_FIRMWARE_BUILD, status, get_status_message(status)); - } - } - return (status); // makes it possible to inline, e.g: return(rpt_exception(status)); + if (status != STAT_OK) { // makes it possible to call exception reports w/o checking status value + if (js.json_syntax == JSON_SYNTAX_RELAXED) { + printf_P(PSTR("{er:{fb:%0.2f,st:%d,msg:\"%s\"}}\n"), + TINYG_FIRMWARE_BUILD, status, get_status_message(status)); + } else { + printf_P(PSTR("{\"er\":{\"fb\":%0.2f,\"st\":%d,\"msg\":\"%s\"}}\n"), + TINYG_FIRMWARE_BUILD, status, get_status_message(status)); + } + } + return status; // makes it possible to inline, e.g: return(rpt_exception(status)); } /* - * rpt_er() - send a bogus exception report for testing purposes (it's not real) + * rpt_er() - send a bogus exception report for testing purposes (it's not real) */ stat_t rpt_er(nvObj_t *nv) { - return(rpt_exception(STAT_GENERIC_EXCEPTION_REPORT)); // bogus exception report for testing + return(rpt_exception(STAT_GENERIC_EXCEPTION_REPORT)); // bogus exception report for testing } /**** Application Messages ********************************************************* - * rpt_print_initializing_message() - initializing configs from hard-coded profile + * rpt_print_initializing_message() - initializing configs from hard-coded profile * rpt_print_loading_configs_message() - loading configs from EEPROM * rpt_print_system_ready_message() - system ready message * - * These messages are always in JSON format to allow UIs to sync + * These messages are always in JSON format to allow UIs to sync */ //void _startup_helper(stat_t status, const char_t *msg) void _startup_helper(stat_t status, const char *msg) { #ifndef __SUPPRESS_STARTUP_MESSAGES - js.json_footer_depth = JSON_FOOTER_DEPTH; //++++ temporary until changeover is complete - nv_reset_nv_list(); - nv_add_object((const char_t *)"fv"); // firmware version - nv_add_object((const char_t *)"fb"); // firmware build - nv_add_object((const char_t *)"hp"); // hardware platform - nv_add_object((const char_t *)"hv"); // hardware version - nv_add_object((const char_t *)"id"); // hardware ID - nv_add_string((const char_t *)"msg", pstr2str(msg)); // startup message - json_print_response(status); + js.json_footer_depth = JSON_FOOTER_DEPTH; //++++ temporary until changeover is complete + nv_reset_nv_list(); + nv_add_object((const char_t *)"fv"); // firmware version + nv_add_object((const char_t *)"fb"); // firmware build + nv_add_object((const char_t *)"hp"); // hardware platform + nv_add_object((const char_t *)"hv"); // hardware version + nv_add_object((const char_t *)"id"); // hardware ID + nv_add_string((const char_t *)"msg", pstr2str(msg)); // startup message + json_print_response(status); #endif } -void rpt_print_initializing_message(void) +void rpt_print_initializing_message() { - _startup_helper(STAT_INITIALIZING, PSTR(INIT_MESSAGE)); + _startup_helper(STAT_INITIALIZING, PSTR(INIT_MESSAGE)); } -void rpt_print_loading_configs_message(void) +void rpt_print_loading_configs_message() { - _startup_helper(STAT_INITIALIZING, PSTR("Loading configs from EEPROM")); + _startup_helper(STAT_INITIALIZING, PSTR("Loading configs from EEPROM")); } -void rpt_print_system_ready_message(void) +void rpt_print_system_ready_message() { - _startup_helper(STAT_OK, PSTR("SYSTEM READY")); - if (cfg.comm_mode == TEXT_MODE) + _startup_helper(STAT_OK, PSTR("SYSTEM READY")); + if (cfg.comm_mode == TEXT_MODE) text_response(STAT_OK, (char_t *)""); // prompt } /***************************************************************************** * Status Reports * - * Status report behaviors + * Status report behaviors * - * Configuration: + * Configuration: * - * Status reports are configurable only from JSON. SRs are configured - * by sending a status report SET object, e.g: + * Status reports are configurable only from JSON. SRs are configured + * by sending a status report SET object, e.g: * - * {"sr":{"line":true,"posx":true,"posy":true....."motm":true,"stat":true}} + * {"sr":{"line":true,"posx":true,"posy":true....."motm":true,"stat":true}} * - * Status report formats: The following formats exist for status reports: + * Status report formats: The following formats exist for status reports: * - * - JSON format: Returns a JSON object as above, but with the values filled in. - * In JSON form all values are returned as numeric values or enumerations. - * E.g. "posx" is returned as 124.523 and "unit" is returned as 0 for - * inches (G20) and 1 for mm (G21). + * - JSON format: Returns a JSON object as above, but with the values filled in. + * In JSON form all values are returned as numeric values or enumerations. + * E.g. "posx" is returned as 124.523 and "unit" is returned as 0 for + * inches (G20) and 1 for mm (G21). * - * - CSV format: Returns a single line of comma separated token:value pairs. - * Values are returned as numeric values or English text. - * E.g. "posx" is still returned as 124.523 but "unit" is returned as - * "inch" for inches (G20) and "mm" for mm (G21). + * - CSV format: Returns a single line of comma separated token:value pairs. + * Values are returned as numeric values or English text. + * E.g. "posx" is still returned as 124.523 but "unit" is returned as + * "inch" for inches (G20) and "mm" for mm (G21). * - * - Multi-line format: Returns a multi-line report where each value occupies - * one line. Each line contains explanatory English text. Enumerated values are - * returned as English text as per CSV form. + * - Multi-line format: Returns a multi-line report where each value occupies + * one line. Each line contains explanatory English text. Enumerated values are + * returned as English text as per CSV form. * - * Status report invocation: Status reports can be invoked in the following ways: + * Status report invocation: Status reports can be invoked in the following ways: * - * - Ad-hoc request in JSON mode. Issue {"sr":""} (or equivalent). Returns a - * JSON format report (wrapped in a response header, of course). + * - Ad-hoc request in JSON mode. Issue {"sr":""} (or equivalent). Returns a + * JSON format report (wrapped in a response header, of course). * - * - Automatic status reports in JSON mode. Returns JSON format reports - * according to "si" setting. + * - Automatic status reports in JSON mode. Returns JSON format reports + * according to "si" setting. * - * - Ad-hoc request in text mode. Triggered by sending ?. Returns status - * report in multi-line format. Additionally, a line starting with ? will put - * the system into text mode. + * - Ad-hoc request in text mode. Triggered by sending ?. Returns status + * report in multi-line format. Additionally, a line starting with ? will put + * the system into text mode. * - * - Automatic status reports in text mode return CSV format according to si setting + * - Automatic status reports in text mode return CSV format according to si setting */ -static stat_t _populate_unfiltered_status_report(void); -static uint8_t _populate_filtered_status_report(void); +static stat_t _populate_unfiltered_status_report(); +static uint8_t _populate_filtered_status_report(); uint8_t _is_stat(nvObj_t *nv) { - char_t tok[TOKEN_LEN+1]; + char_t tok[TOKEN_LEN+1]; - GET_TOKEN_STRING(nv->value, tok); - if (strcmp(tok, "stat") == 0) { return (true);} - return (false); + GET_TOKEN_STRING(nv->value, tok); + if (strcmp(tok, "stat") == 0) { return true;} + return false; } /* * sr_init_status_report() * - * Call this function to completely re-initialize the status report - * Sets SR list to hard-coded defaults and re-initializes SR values in NVM + * Call this function to completely re-initialize the status report + * Sets SR list to hard-coded defaults and re-initializes SR values in NVM */ void sr_init_status_report() { - nvObj_t *nv = nv_reset_nv_list(); // used for status report persistence locations - sr.status_report_requested = false; - char_t sr_defaults[NV_STATUS_REPORT_LEN][TOKEN_LEN+1] = { STATUS_REPORT_DEFAULTS }; // see settings.h - nv->index = nv_get_index((const char_t *)"", (const char_t *)"se00"); // set first SR persistence index - sr.stat_index = 0; - - for (uint8_t i=0; i < NV_STATUS_REPORT_LEN ; i++) { - if (sr_defaults[i][0] == NUL) break; // quit on first blank array entry - sr.status_report_value[i] = -1234567; // pre-load values with an unlikely number - nv->value = nv_get_index((const char_t *)"", sr_defaults[i]);// load the index for the SR element - if (nv->value == NO_MATCH) { - rpt_exception(STAT_BAD_STATUS_REPORT_SETTING); // trap mis-configured profile settings - return; - } - if (_is_stat(nv) == true) - sr.stat_index = nv->value; // identify index for 'stat' if status is in the report - nv_set(nv); - nv_persist(nv); // conditionally persist - automatic by nv_persist() - nv->index++; // increment SR NVM index - } + nvObj_t *nv = nv_reset_nv_list(); // used for status report persistence locations + sr.status_report_requested = false; + char_t sr_defaults[NV_STATUS_REPORT_LEN][TOKEN_LEN+1] = { STATUS_REPORT_DEFAULTS }; // see settings.h + nv->index = nv_get_index((const char_t *)"", (const char_t *)"se00"); // set first SR persistence index + sr.stat_index = 0; + + for (uint8_t i=0; i < NV_STATUS_REPORT_LEN ; i++) { + if (sr_defaults[i][0] == 0) break; // quit on first blank array entry + sr.status_report_value[i] = -1234567; // pre-load values with an unlikely number + nv->value = nv_get_index((const char_t *)"", sr_defaults[i]);// load the index for the SR element + if (nv->value == NO_MATCH) { + rpt_exception(STAT_BAD_STATUS_REPORT_SETTING); // trap mis-configured profile settings + return; + } + if (_is_stat(nv) == true) + sr.stat_index = nv->value; // identify index for 'stat' if status is in the report + nv_set(nv); + nv_persist(nv); // conditionally persist - automatic by nv_persist() + nv->index++; // increment SR NVM index + } } /* * sr_set_status_report() - interpret an SR setup string and return current report * - * Note: By the time this function is called any unrecognized tokens have been detected and - * rejected by the JSON or text parser. In other words, it should never get to here if - * there is an unrecognized token in the SR string. + * Note: By the time this function is called any unrecognized tokens have been detected and + * rejected by the JSON or text parser. In other words, it should never get to here if + * there is an unrecognized token in the SR string. */ stat_t sr_set_status_report(nvObj_t *nv) { - uint8_t elements = 0; - index_t status_report_list[NV_STATUS_REPORT_LEN]; - memset(status_report_list, 0, sizeof(status_report_list)); - index_t sr_start = nv_get_index((const char_t *)"",(const char_t *)"se00");// set first SR persistence index - - for (uint8_t i=0; inx) == NULL) || (nv->valuetype == TYPE_EMPTY)) break; - if ((nv->valuetype == TYPE_BOOL) && (fp_TRUE(nv->value))) { - status_report_list[i] = nv->index; - nv->value = nv->index; // persist the index as the value - nv->index = sr_start + i; // index of the SR persistence location - nv_persist(nv); - elements++; - } else { - return (STAT_UNRECOGNIZED_NAME); - } - } - if (elements == 0) - return (STAT_INVALID_OR_MALFORMED_COMMAND); - memcpy(sr.status_report_list, status_report_list, sizeof(status_report_list)); - return(_populate_unfiltered_status_report()); // return current values + uint8_t elements = 0; + index_t status_report_list[NV_STATUS_REPORT_LEN]; + memset(status_report_list, 0, sizeof(status_report_list)); + index_t sr_start = nv_get_index((const char_t *)"",(const char_t *)"se00");// set first SR persistence index + + for (uint8_t i=0; inx) == 0) || (nv->valuetype == TYPE_EMPTY)) break; + if ((nv->valuetype == TYPE_BOOL) && (fp_TRUE(nv->value))) { + status_report_list[i] = nv->index; + nv->value = nv->index; // persist the index as the value + nv->index = sr_start + i; // index of the SR persistence location + nv_persist(nv); + elements++; + } else { + return STAT_UNRECOGNIZED_NAME; + } + } + if (elements == 0) + return STAT_INVALID_OR_MALFORMED_COMMAND; + memcpy(sr.status_report_list, status_report_list, sizeof(status_report_list)); + return(_populate_unfiltered_status_report()); // return current values } /* - * sr_request_status_report() - request a status report to run after minimum interval - * sr_status_report_callback() - main loop callback to send a report if one is ready + * sr_request_status_report() - request a status report to run after minimum interval + * sr_status_report_callback() - main loop callback to send a report if one is ready * - * Status reports can be request from a number of sources including: - * - direct request from command line in the form of ? or {"sr:""} - * - timed requests during machining cycle - * - filtered request after each Gcode block + * Status reports can be request from a number of sources including: + * - direct request from command line in the form of ? or {"sr:""} + * - timed requests during machining cycle + * - filtered request after each Gcode block * - * Status reports are generally returned with minimal delay (from the controller callback), - * but will not be provided more frequently than the status report interval + * Status reports are generally returned with minimal delay (from the controller callback), + * but will not be provided more frequently than the status report interval */ stat_t sr_request_status_report(uint8_t request_type) { if (request_type == SR_IMMEDIATE_REQUEST) { - sr.status_report_systick = SysTickTimer_getValue(); - } - if ((request_type == SR_TIMED_REQUEST) && (sr.status_report_requested == false)) { - sr.status_report_systick = SysTickTimer_getValue() + sr.status_report_interval; - } + sr.status_report_systick = SysTickTimer_getValue(); + } + if ((request_type == SR_TIMED_REQUEST) && (sr.status_report_requested == false)) { + sr.status_report_systick = SysTickTimer_getValue() + sr.status_report_interval; + } sr.status_report_requested = true; - return (STAT_OK); + return STAT_OK; } -stat_t sr_status_report_callback() // called by controller dispatcher +stat_t sr_status_report_callback() // called by controller dispatcher { #ifdef __SUPPRESS_STATUS_REPORTS - return (STAT_NOOP); + return STAT_NOOP; #endif - if (sr.status_report_verbosity == SR_OFF) - return (STAT_NOOP); + if (sr.status_report_verbosity == SR_OFF) + return STAT_NOOP; - if (sr.status_report_requested == false) - return (STAT_NOOP); + if (sr.status_report_requested == false) + return STAT_NOOP; if (SysTickTimer_getValue() < sr.status_report_systick) - return (STAT_NOOP); - - sr.status_report_requested = false; // disable reports until requested again - - if (sr.status_report_verbosity == SR_VERBOSE) { - _populate_unfiltered_status_report(); - } else { - if (_populate_filtered_status_report() == false) { // no new data - return (STAT_OK); - } - } - nv_print_list(STAT_OK, TEXT_INLINE_PAIRS, JSON_OBJECT_FORMAT); - return (STAT_OK); + return STAT_NOOP; + + sr.status_report_requested = false; // disable reports until requested again + + if (sr.status_report_verbosity == SR_VERBOSE) { + _populate_unfiltered_status_report(); + } else { + if (_populate_filtered_status_report() == false) { // no new data + return STAT_OK; + } + } + nv_print_list(STAT_OK, TEXT_INLINE_PAIRS, JSON_OBJECT_FORMAT); + return STAT_OK; } /* @@ -286,107 +286,107 @@ stat_t sr_status_report_callback() // called by controller dispatcher */ stat_t sr_run_text_status_report() { - _populate_unfiltered_status_report(); - nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); - return (STAT_OK); + _populate_unfiltered_status_report(); + nv_print_list(STAT_OK, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); + return STAT_OK; } /* * _populate_unfiltered_status_report() - populate nvObj body with status values * - * Designed to be run as a response; i.e. have a "r" header and a footer. + * Designed to be run as a response; i.e. have a "r" header and a footer. */ static stat_t _populate_unfiltered_status_report() { - const char_t sr_str[] = "sr"; - char_t tmp[TOKEN_LEN+1]; - nvObj_t *nv = nv_reset_nv_list(); // sets *nv to the start of the body - - nv->valuetype = TYPE_PARENT; // setup the parent object (no length checking required) - strcpy(nv->token, sr_str); - nv->index = nv_get_index((const char_t *)"", sr_str);// set the index - may be needed by calling function - nv = nv->nx; // no need to check for NULL as list has just been reset - - for (uint8_t i=0; iindex = sr.status_report_list[i]) == 0) { break;} - nv_get_nvObj(nv); - - strcpy(tmp, nv->group); // flatten out groups - WARNING - you cannot use strncpy here... - strcat(tmp, nv->token); - strcpy(nv->token, tmp); //...or here. - - if ((nv = nv->nx) == NULL) - return (cm_hard_alarm(STAT_BUFFER_FULL_FATAL)); // should never be NULL unless SR length exceeds available buffer array - } - return (STAT_OK); + const char_t sr_str[] = "sr"; + char_t tmp[TOKEN_LEN+1]; + nvObj_t *nv = nv_reset_nv_list(); // sets *nv to the start of the body + + nv->valuetype = TYPE_PARENT; // setup the parent object (no length checking required) + strcpy(nv->token, sr_str); + nv->index = nv_get_index((const char_t *)"", sr_str);// set the index - may be needed by calling function + nv = nv->nx; // no need to check for 0 as list has just been reset + + for (uint8_t i=0; iindex = sr.status_report_list[i]) == 0) { break;} + nv_get_nvObj(nv); + + strcpy(tmp, nv->group); // flatten out groups - WARNING - you cannot use strncpy here... + strcat(tmp, nv->token); + strcpy(nv->token, tmp); //...or here. + + if ((nv = nv->nx) == 0) + return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // should never be 0 unless SR length exceeds available buffer array + } + return STAT_OK; } /* * _populate_filtered_status_report() - populate nvObj body with status values * - * Designed to be displayed as a JSON object; i;e; no footer or header - * Returns 'true' if the report has new data, 'false' if there is nothing to report. + * Designed to be displayed as a JSON object; i;e; no footer or header + * Returns 'true' if the report has new data, 'false' if there is nothing to report. * - * NOTE: Unlike sr_populate_unfiltered_status_report(), this function does NOT set - * the SR index, which is a relatively expensive operation. In current use this - * doesn't matter, but if the caller assumes its set it may lead to a side-effect (bug) + * NOTE: Unlike sr_populate_unfiltered_status_report(), this function does NOT set + * the SR index, which is a relatively expensive operation. In current use this + * doesn't matter, but if the caller assumes its set it may lead to a side-effect (bug) * - * NOTE: Room for improvement - look up the SR index initially and cache it, use the - * cached value for all remaining reports. + * NOTE: Room for improvement - look up the SR index initially and cache it, use the + * cached value for all remaining reports. */ static uint8_t _populate_filtered_status_report() { - const char_t sr_str[] = "sr"; - uint8_t has_data = false; - char_t tmp[TOKEN_LEN+1]; - nvObj_t *nv = nv_reset_nv_list(); // sets nv to the start of the body - - nv->valuetype = TYPE_PARENT; // setup the parent object (no need to length check the copy) - strcpy(nv->token, sr_str); -// nv->index = nv_get_index((const char_t *)"", sr_str);// OMITTED - set the index - may be needed by calling function - nv = nv->nx; // no need to check for NULL as list has just been reset - - for (uint8_t i=0; iindex = sr.status_report_list[i]) == 0) { break;} - - nv_get_nvObj(nv); - // do not report values that have not changed... - // ...except for stat=3 (STOP), which is an exception - if (fp_EQ(nv->value, sr.status_report_value[i])) { -// if (nv->index != sr.stat_index) { -// if (fp_EQ(nv->value, COMBINED_PROGRAM_STOP)) { - nv->valuetype = TYPE_EMPTY; - continue; -// } -// } - // report anything that has changed - } else { - strcpy(tmp, nv->group); // flatten out groups - WARNING - you cannot use strncpy here... - strcat(tmp, nv->token); - strcpy(nv->token, tmp); //...or here. - sr.status_report_value[i] = nv->value; - if ((nv = nv->nx) == NULL) return (false); // should never be NULL unless SR length exceeds available buffer array - has_data = true; - } - } - return (has_data); + const char_t sr_str[] = "sr"; + uint8_t has_data = false; + char_t tmp[TOKEN_LEN+1]; + nvObj_t *nv = nv_reset_nv_list(); // sets nv to the start of the body + + nv->valuetype = TYPE_PARENT; // setup the parent object (no need to length check the copy) + strcpy(nv->token, sr_str); +// nv->index = nv_get_index((const char_t *)"", sr_str);// OMITTED - set the index - may be needed by calling function + nv = nv->nx; // no need to check for 0 as list has just been reset + + for (uint8_t i=0; iindex = sr.status_report_list[i]) == 0) { break;} + + nv_get_nvObj(nv); + // do not report values that have not changed... + // ...except for stat=3 (STOP), which is an exception + if (fp_EQ(nv->value, sr.status_report_value[i])) { +// if (nv->index != sr.stat_index) { +// if (fp_EQ(nv->value, COMBINED_PROGRAM_STOP)) { + nv->valuetype = TYPE_EMPTY; + continue; +// } +// } + // report anything that has changed + } else { + strcpy(tmp, nv->group); // flatten out groups - WARNING - you cannot use strncpy here... + strcat(tmp, nv->token); + strcpy(nv->token, tmp); //...or here. + sr.status_report_value[i] = nv->value; + if ((nv = nv->nx) == 0) return false; // should never be 0 unless SR length exceeds available buffer array + has_data = true; + } + } + return has_data; } /* * Wrappers and Setters - for calling from nvArray table * - * sr_get() - run status report - * sr_set() - set status report elements - * sr_set_si() - set status report interval + * sr_get() - run status report + * sr_set() - set status report elements + * sr_set_si() - set status report interval */ -stat_t sr_get(nvObj_t *nv) { return (_populate_unfiltered_status_report());} -stat_t sr_set(nvObj_t *nv) { return (sr_set_status_report(nv));} +stat_t sr_get(nvObj_t *nv) { return _populate_unfiltered_status_report();} +stat_t sr_set(nvObj_t *nv) { return sr_set_status_report(nv);} stat_t sr_set_si(nvObj_t *nv) { - if (nv->value < STATUS_REPORT_MIN_MS) { nv->value = STATUS_REPORT_MIN_MS;} - sr.status_report_interval = (uint32_t)nv->value; - return(STAT_OK); + if (nv->value < STATUS_REPORT_MIN_MS) { nv->value = STATUS_REPORT_MIN_MS;} + sr.status_report_interval = (uint32_t)nv->value; + return(STAT_OK); } /********************* @@ -407,110 +407,110 @@ void sr_print_sv(nvObj_t *nv) { text_print_ui8(nv, fmt_sv);} /***************************************************************************** * Queue Reports * - * Queue reports can report three values: - * - qr queue depth - # of buffers availabel in planner queue - * - qi buffers added to planner queue since las report - * - qo buffers removed from planner queue since last report + * Queue reports can report three values: + * - qr queue depth - # of buffers availabel in planner queue + * - qi buffers added to planner queue since las report + * - qo buffers removed from planner queue since last report * - * A QR_SINGLE report returns qr only. A QR_TRIPLE returns all 3 values + * A QR_SINGLE report returns qr only. A QR_TRIPLE returns all 3 values * - * There are 2 ways to get queue reports: + * There are 2 ways to get queue reports: * - * 1. Enable single or triple queue reports using the QV variable. This will - * return a queue report every time the buffer depth changes + * 1. Enable single or triple queue reports using the QV variable. This will + * return a queue report every time the buffer depth changes * - * 2. Add qr, qi and qo (or some combination) to the status report. This will - * return queue report data when status reports are generated. + * 2. Add qr, qi and qo (or some combination) to the status report. This will + * return queue report data when status reports are generated. */ /* * qr_init_queue_report() - initialize or clear queue report values */ void qr_init_queue_report() { - qr.queue_report_requested = false; - qr.buffers_added = 0; - qr.buffers_removed = 0; - qr.init_tick = SysTickTimer_getValue(); + qr.queue_report_requested = false; + qr.buffers_added = 0; + qr.buffers_removed = 0; + qr.init_tick = SysTickTimer_getValue(); } /* * qr_request_queue_report() - request a queue report * - * Requests a queue report and also records the buffers added and removed - * since the last init (usually re-initted when a report is generated). + * Requests a queue report and also records the buffers added and removed + * since the last init (usually re-initted when a report is generated). */ void qr_request_queue_report(int8_t buffers) { - // get buffer depth and added/removed count - qr.buffers_available = mp_get_planner_buffers_available(); - if (buffers > 0) { - qr.buffers_added += buffers; - } else { - qr.buffers_removed -= buffers; - } - - // time-throttle requests while generating arcs - qr.motion_mode = cm_get_motion_mode(ACTIVE_MODEL); - if ((qr.motion_mode == MOTION_MODE_CW_ARC) || (qr.motion_mode == MOTION_MODE_CCW_ARC)) { - uint32_t tick = SysTickTimer_getValue(); - if (tick - qr.init_tick < MIN_ARC_QR_INTERVAL) { - qr.queue_report_requested = false; - return; - } - } - - // either return or request a report - if (qr.queue_report_verbosity != QR_OFF) { - qr.queue_report_requested = true; - } + // get buffer depth and added/removed count + qr.buffers_available = mp_get_planner_buffers_available(); + if (buffers > 0) { + qr.buffers_added += buffers; + } else { + qr.buffers_removed -= buffers; + } + + // time-throttle requests while generating arcs + qr.motion_mode = cm_get_motion_mode(ACTIVE_MODEL); + if ((qr.motion_mode == MOTION_MODE_CW_ARC) || (qr.motion_mode == MOTION_MODE_CCW_ARC)) { + uint32_t tick = SysTickTimer_getValue(); + if (tick - qr.init_tick < MIN_ARC_QR_INTERVAL) { + qr.queue_report_requested = false; + return; + } + } + + // either return or request a report + if (qr.queue_report_verbosity != QR_OFF) { + qr.queue_report_requested = true; + } } /* * qr_queue_report_callback() - generate a queue report if one has been requested */ -stat_t qr_queue_report_callback() // called by controller dispatcher +stat_t qr_queue_report_callback() // called by controller dispatcher { #ifdef __SUPPRESS_QUEUE_REPORTS - return (STAT_NOOP); + return STAT_NOOP; #endif - if (qr.queue_report_verbosity == QR_OFF) - return (STAT_NOOP); - - if (qr.queue_report_requested == false) - return (STAT_NOOP); - - qr.queue_report_requested = false; - - if (cfg.comm_mode == TEXT_MODE) { - if (qr.queue_report_verbosity == QR_SINGLE) { - fprintf(stderr, "qr:%d\n", qr.buffers_available); - } else { - fprintf(stderr, "qr:%d, qi:%d, qo:%d\n", qr.buffers_available,qr.buffers_added,qr.buffers_removed); - } - - } else if (js.json_syntax == JSON_SYNTAX_RELAXED) { - if (qr.queue_report_verbosity == QR_SINGLE) { - fprintf(stderr, "{qr:%d}\n", qr.buffers_available); - } else { - fprintf(stderr, "{qr:%d,qi:%d,qo:%d}\n", qr.buffers_available, qr.buffers_added,qr.buffers_removed); - } - - } else { - if (qr.queue_report_verbosity == QR_SINGLE) { - fprintf(stderr, "{\"qr\":%d}\n", qr.buffers_available); - } else { - fprintf(stderr, "{\"qr\":%d,\"qi\":%d,\"qo\":%d}\n", qr.buffers_available, qr.buffers_added,qr.buffers_removed); - } - } - qr_init_queue_report(); - return (STAT_OK); + if (qr.queue_report_verbosity == QR_OFF) + return STAT_NOOP; + + if (qr.queue_report_requested == false) + return STAT_NOOP; + + qr.queue_report_requested = false; + + if (cfg.comm_mode == TEXT_MODE) { + if (qr.queue_report_verbosity == QR_SINGLE) { + fprintf(stderr, "qr:%d\n", qr.buffers_available); + } else { + fprintf(stderr, "qr:%d, qi:%d, qo:%d\n", qr.buffers_available,qr.buffers_added,qr.buffers_removed); + } + + } else if (js.json_syntax == JSON_SYNTAX_RELAXED) { + if (qr.queue_report_verbosity == QR_SINGLE) { + fprintf(stderr, "{qr:%d}\n", qr.buffers_available); + } else { + fprintf(stderr, "{qr:%d,qi:%d,qo:%d}\n", qr.buffers_available, qr.buffers_added,qr.buffers_removed); + } + + } else { + if (qr.queue_report_verbosity == QR_SINGLE) { + fprintf(stderr, "{\"qr\":%d}\n", qr.buffers_available); + } else { + fprintf(stderr, "{\"qr\":%d,\"qi\":%d,\"qo\":%d}\n", qr.buffers_available, qr.buffers_added,qr.buffers_removed); + } + } + qr_init_queue_report(); + return STAT_OK; } /* * rx_request_rx_report() - request an update on usb serial buffer space available */ -void rx_request_rx_report(void) { +void rx_request_rx_report() { rx.rx_report_requested = true; rx.space_available = xio_get_usb_rx_free(); } @@ -518,30 +518,30 @@ void rx_request_rx_report(void) { /* * rx_report_callback() - send rx report if one has been requested */ -stat_t rx_report_callback(void) { +stat_t rx_report_callback() { if (!rx.rx_report_requested) - return (STAT_NOOP); + return STAT_NOOP; rx.rx_report_requested = false; fprintf(stderr, "{\"rx\":%d}\n", rx.space_available); - return (STAT_OK); + return STAT_OK; } /* Alternate Formulation for a Single report - using nvObj list - // get a clean nv object -// nvObj_t *nv = nv_reset_nv_list(); // normally you do a list reset but the following is more time efficient - nvObj_t *nv = nv_body; - nv_reset_nv(nv); - nv->nx = NULL; // terminate the list - - // make a qr object and print it - sprintf_P(nv->token, PSTR("qr")); - nv->value = qr.buffers_available; - nv->valuetype = TYPE_INTEGER; - nv_print_list(STAT_OK, TEXT_INLINE_PAIRS, JSON_OBJECT_FORMAT); - return (STAT_OK); + // get a clean nv object +// nvObj_t *nv = nv_reset_nv_list(); // normally you do a list reset but the following is more time efficient + nvObj_t *nv = nv_body; + nv_reset_nv(nv); + nv->nx = 0; // terminate the list + + // make a qr object and print it + sprintf_P(nv->token, PSTR("qr")); + nv->value = qr.buffers_available; + nv->valuetype = TYPE_INTEGER; + nv_print_list(STAT_OK, TEXT_INLINE_PAIRS, JSON_OBJECT_FORMAT); + return STAT_OK; */ /* @@ -553,98 +553,98 @@ stat_t rx_report_callback(void) { */ stat_t qr_get(nvObj_t *nv) { - nv->value = (float)mp_get_planner_buffers_available(); // ensure that manually requested QR count is always up to date - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); + nv->value = (float)mp_get_planner_buffers_available(); // ensure that manually requested QR count is always up to date + nv->valuetype = TYPE_INTEGER; + return STAT_OK; } stat_t qi_get(nvObj_t *nv) { - nv->value = (float)qr.buffers_added; - nv->valuetype = TYPE_INTEGER; - qr.buffers_added = 0; // reset it - return (STAT_OK); + nv->value = (float)qr.buffers_added; + nv->valuetype = TYPE_INTEGER; + qr.buffers_added = 0; // reset it + return STAT_OK; } stat_t qo_get(nvObj_t *nv) { - nv->value = (float)qr.buffers_removed; - nv->valuetype = TYPE_INTEGER; - qr.buffers_removed = 0; // reset it - return (STAT_OK); + nv->value = (float)qr.buffers_removed; + nv->valuetype = TYPE_INTEGER; + qr.buffers_removed = 0; // reset it + return STAT_OK; } /***************************************************************************** * JOB ID REPORTS * - * job_populate_job_report() - * job_set_job_report() - * job_report_callback() - * job_get() - * job_set() - * job_print_job() + * job_populate_job_report() + * job_set_job_report() + * job_report_callback() + * job_get() + * job_set() + * job_print_job() */ stat_t job_populate_job_report() { - const char_t job_str[] = "job"; - char_t tmp[TOKEN_LEN+1]; - nvObj_t *nv = nv_reset_nv_list(); // sets *nv to the start of the body + const char_t job_str[] = "job"; + char_t tmp[TOKEN_LEN+1]; + nvObj_t *nv = nv_reset_nv_list(); // sets *nv to the start of the body - nv->valuetype = TYPE_PARENT; // setup the parent object - strcpy(nv->token, job_str); + nv->valuetype = TYPE_PARENT; // setup the parent object + strcpy(nv->token, job_str); - //nv->index = nv_get_index((const char_t *)"", job_str);// set the index - may be needed by calling function - nv = nv->nx; // no need to check for NULL as list has just been reset + //nv->index = nv_get_index((const char_t *)"", job_str);// set the index - may be needed by calling function + nv = nv->nx; // no need to check for 0 as list has just been reset - index_t job_start = nv_get_index((const char_t *)"",(const char_t *)"job1");// set first job persistence index - for (uint8_t i=0; i<4; i++) { + index_t job_start = nv_get_index((const char_t *)"",(const char_t *)"job1");// set first job persistence index + for (uint8_t i=0; i<4; i++) { - nv->index = job_start + i; - nv_get_nvObj(nv); + nv->index = job_start + i; + nv_get_nvObj(nv); - strcpy(tmp, nv->group); // concatenate groups and tokens - do NOT use strncpy() - strcat(tmp, nv->token); - strcpy(nv->token, tmp); + strcpy(tmp, nv->group); // concatenate groups and tokens - do NOT use strncpy() + strcat(tmp, nv->token); + strcpy(nv->token, tmp); - if ((nv = nv->nx) == NULL) - return (STAT_OK); // should never be NULL unless SR length exceeds available buffer array - } - return (STAT_OK); + if ((nv = nv->nx) == 0) + return STAT_OK; // should never be 0 unless SR length exceeds available buffer array + } + return STAT_OK; } stat_t job_set_job_report(nvObj_t *nv) { - index_t job_start = nv_get_index((const char_t *)"",(const char_t *)"job1");// set first job persistence index - - for (uint8_t i=0; i<4; i++) { - if (((nv = nv->nx) == NULL) || (nv->valuetype == TYPE_EMPTY)) { break;} - if (nv->valuetype == TYPE_INTEGER) { - cs.job_id[i] = nv->value; - nv->index = job_start + i; // index of the SR persistence location - nv_persist(nv); - } else { - return (STAT_UNSUPPORTED_TYPE); - } - } - job_populate_job_report(); // return current values - return (STAT_OK); + index_t job_start = nv_get_index((const char_t *)"",(const char_t *)"job1");// set first job persistence index + + for (uint8_t i=0; i<4; i++) { + if (((nv = nv->nx) == 0) || (nv->valuetype == TYPE_EMPTY)) { break;} + if (nv->valuetype == TYPE_INTEGER) { + cs.job_id[i] = nv->value; + nv->index = job_start + i; // index of the SR persistence location + nv_persist(nv); + } else { + return STAT_UNSUPPORTED_TYPE; + } + } + job_populate_job_report(); // return current values + return STAT_OK; } uint8_t job_report_callback() { - if (cfg.comm_mode == TEXT_MODE) { - // no-op, job_ids are client app state - } else if (js.json_syntax == JSON_SYNTAX_RELAXED) { - fprintf(stderr, "{job:[%lu,%lu,%lu,%lu]}\n", cs.job_id[0], cs.job_id[1], cs.job_id[2], cs.job_id[3] ); - } else { - fprintf(stderr, "{\"job\":[%lu,%lu,%lu,%lu]}\n", cs.job_id[0], cs.job_id[1], cs.job_id[2], cs.job_id[3] ); - //job_clear_report(); - } - return (STAT_OK); + if (cfg.comm_mode == TEXT_MODE) { + // no-op, job_ids are client app state + } else if (js.json_syntax == JSON_SYNTAX_RELAXED) { + fprintf(stderr, "{job:[%lu,%lu,%lu,%lu]}\n", cs.job_id[0], cs.job_id[1], cs.job_id[2], cs.job_id[3] ); + } else { + fprintf(stderr, "{\"job\":[%lu,%lu,%lu,%lu]}\n", cs.job_id[0], cs.job_id[1], cs.job_id[2], cs.job_id[3] ); + //job_clear_report(); + } + return STAT_OK; } -stat_t job_get(nvObj_t *nv) { return (job_populate_job_report());} -stat_t job_set(nvObj_t *nv) { return (job_set_job_report(nv));} +stat_t job_get(nvObj_t *nv) { return job_populate_job_report();} +stat_t job_set(nvObj_t *nv) { return job_set_job_report(nv);} void job_print_job(nvObj_t *nv) { job_populate_job_report();} /********************* diff --git a/src/report.h b/src/report.h index 0c3960e..595717c 100755 --- a/src/report.h +++ b/src/report.h @@ -31,56 +31,56 @@ /**** Configs, Definitions and Structures ****/ // // Notes: -// - The NV_STATUS_REPORT_LEN define is in config.h -// - The status report defaults can be found in settings.h +// - The NV_STATUS_REPORT_LEN define is in config.h +// - The status report defaults can be found in settings.h -#define MIN_ARC_QR_INTERVAL 200 // minimum interval between QRs during arc generation (in system ticks) +#define MIN_ARC_QR_INTERVAL 200 // minimum interval between QRs during arc generation (in system ticks) -enum srVerbosity { // status report enable and verbosity - SR_OFF = 0, // no reports - SR_FILTERED, // reports only values that have changed from the last report - SR_VERBOSE // reports all values specified +enum srVerbosity { // status report enable and verbosity + SR_OFF = 0, // no reports + SR_FILTERED, // reports only values that have changed from the last report + SR_VERBOSE // reports all values specified }; enum cmStatusReportRequest { - SR_TIMED_REQUEST = 0, // request a status report at next timer interval - SR_IMMEDIATE_REQUEST // request a status report ASAP + SR_TIMED_REQUEST = 0, // request a status report at next timer interval + SR_IMMEDIATE_REQUEST // request a status report ASAP }; -enum qrVerbosity { // planner queue enable and verbosity - QR_OFF = 0, // no response is provided - QR_SINGLE, // queue depth reported - QR_TRIPLE // queue depth reported for buffers, buffers added, buffered removed +enum qrVerbosity { // planner queue enable and verbosity + QR_OFF = 0, // no response is provided + QR_SINGLE, // queue depth reported + QR_TRIPLE // queue depth reported for buffers, buffers added, buffered removed }; typedef struct srSingleton { - /*** config values (PUBLIC) ***/ - uint8_t status_report_verbosity; - uint32_t status_report_interval; // in milliseconds + /*** config values (PUBLIC) ***/ + uint8_t status_report_verbosity; + uint32_t status_report_interval; // in milliseconds - /*** runtime values (PRIVATE) ***/ - uint8_t status_report_requested; // flag that SR has been requested - uint32_t status_report_systick; // SysTick value for next status report - index_t stat_index; // table index value for stat - determined during initialization - index_t status_report_list[NV_STATUS_REPORT_LEN]; // status report elements to report - float status_report_value[NV_STATUS_REPORT_LEN]; // previous values for filtered reporting + /*** runtime values (PRIVATE) ***/ + uint8_t status_report_requested; // flag that SR has been requested + uint32_t status_report_systick; // SysTick value for next status report + index_t stat_index; // table index value for stat - determined during initialization + index_t status_report_list[NV_STATUS_REPORT_LEN]; // status report elements to report + float status_report_value[NV_STATUS_REPORT_LEN]; // previous values for filtered reporting } srSingleton_t; -typedef struct qrSingleton { // data for queue reports +typedef struct qrSingleton { // data for queue reports - /*** config values (PUBLIC) ***/ - uint8_t queue_report_verbosity; // queue reports enabled and verbosity level + /*** config values (PUBLIC) ***/ + uint8_t queue_report_verbosity; // queue reports enabled and verbosity level - /*** runtime values (PRIVATE) ***/ - uint8_t queue_report_requested; // set to true to request a report - uint8_t buffers_available; // stored buffer depth passed to by callback - uint8_t prev_available; // buffers available at last count - uint16_t buffers_added; // buffers added since last count - uint16_t buffers_removed; // buffers removed since last report - uint8_t motion_mode; // used to detect arc movement - uint32_t init_tick; // time when values were last initialized or cleared + /*** runtime values (PRIVATE) ***/ + uint8_t queue_report_requested; // set to true to request a report + uint8_t buffers_available; // stored buffer depth passed to by callback + uint8_t prev_available; // buffers available at last count + uint16_t buffers_added; // buffers added since last count + uint16_t buffers_removed; // buffers removed since last report + uint8_t motion_mode; // used to detect arc movement + uint32_t init_tick; // time when values were last initialized or cleared } qrSingleton_t; @@ -101,26 +101,26 @@ void rpt_print_message(char *msg); stat_t rpt_exception(uint8_t status); stat_t rpt_er(nvObj_t *nv); -void rpt_print_loading_configs_message(void); -void rpt_print_initializing_message(void); -void rpt_print_system_ready_message(void); +void rpt_print_loading_configs_message(); +void rpt_print_initializing_message(); +void rpt_print_system_ready_message(); -void sr_init_status_report(void); +void sr_init_status_report(); stat_t sr_set_status_report(nvObj_t *nv); stat_t sr_request_status_report(uint8_t request_type); -stat_t sr_status_report_callback(void); -stat_t sr_run_text_status_report(void); +stat_t sr_status_report_callback(); +stat_t sr_run_text_status_report(); stat_t sr_get(nvObj_t *nv); stat_t sr_set(nvObj_t *nv); stat_t sr_set_si(nvObj_t *nv); -void qr_init_queue_report(void); +void qr_init_queue_report(); void qr_request_queue_report(int8_t buffers); -stat_t qr_queue_report_callback(void); +stat_t qr_queue_report_callback(); -void rx_request_rx_report(void); -stat_t rx_report_callback(void); +void rx_request_rx_report(); +stat_t rx_report_callback(); stat_t qr_get(nvObj_t *nv); stat_t qi_get(nvObj_t *nv); @@ -128,23 +128,23 @@ stat_t qo_get(nvObj_t *nv); #ifdef __TEXT_MODE - void sr_print_sr(nvObj_t *nv); - void sr_print_si(nvObj_t *nv); - void sr_print_sv(nvObj_t *nv); - void qr_print_qv(nvObj_t *nv); - void qr_print_qr(nvObj_t *nv); - void qr_print_qi(nvObj_t *nv); - void qr_print_qo(nvObj_t *nv); + void sr_print_sr(nvObj_t *nv); + void sr_print_si(nvObj_t *nv); + void sr_print_sv(nvObj_t *nv); + void qr_print_qv(nvObj_t *nv); + void qr_print_qr(nvObj_t *nv); + void qr_print_qi(nvObj_t *nv); + void qr_print_qo(nvObj_t *nv); #else - #define sr_print_sr tx_print_stub - #define sr_print_si tx_print_stub - #define sr_print_sv tx_print_stub - #define qr_print_qv tx_print_stub - #define qr_print_qr tx_print_stub - #define qr_print_qi tx_print_stub - #define qr_print_qo tx_print_stub + #define sr_print_sr tx_print_stub + #define sr_print_si tx_print_stub + #define sr_print_sv tx_print_stub + #define qr_print_qv tx_print_stub + #define qr_print_qr tx_print_stub + #define qr_print_qi tx_print_stub + #define qr_print_qo tx_print_stub #endif // __TEXT_MODE diff --git a/src/settings.h b/src/settings.h index 9e13a9e..f696f5e 100755 --- a/src/settings.h +++ b/src/settings.h @@ -24,12 +24,12 @@ * 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 values in this file are the default settings that are loaded into a virgin EEPROM, - * and can be changed using the config commands. After initial load the EEPROM values - * (or changed values) are used. +/* The values in this file are the default settings that are loaded into a virgin EEPROM, + * and can be changed using the config commands. After initial load the EEPROM values + * (or changed values) are used. * - * System and hardware settings that you shouldn't need to change are in hardware.h - * Application settings that also shouldn't need to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change are in hardware.h + * Application settings that also shouldn't need to be changed are in tinyg.h */ #ifndef SETTINGS_H_ONCE @@ -41,114 +41,101 @@ // Do not assume these are the effective settings. Check the machine profile // Machine configuration settings -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing -#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on -#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED +#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing +#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on +#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // one of: MOTOR_DISABLED (0) - // MOTOR_ALWAYS_POWERED (1) - // MOTOR_POWERED_IN_CYCLE (2) - // MOTOR_POWERED_ONLY_WHEN_MOVING (3) +#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // one of: MOTOR_DISABLED (0) + // MOTOR_ALWAYS_POWERED (1) + // MOTOR_POWERED_IN_CYCLE (2) + // MOTOR_POWERED_ONLY_WHEN_MOVING (3) -#define MOTOR_IDLE_TIMEOUT 2.00 // seconds to maintain motor at full power before idling +#define MOTOR_IDLE_TIMEOUT 2.00 // seconds to maintain motor at full power before idling // Communications and reporting settings -#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE -#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE -#define NETWORK_MODE NETWORK_STANDALONE +#define COMM_MODE JSON_MODE // one of: TEXT_MODE, JSON_MODE +#define TEXT_VERBOSITY TV_VERBOSE // one of: TV_SILENT, TV_VERBOSE +#define NETWORK_MODE NETWORK_STANDALONE -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT -#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style +#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT +#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE= -#define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum -#define STATUS_REPORT_INTERVAL_MS 500 // milliseconds - set $SV=0 to disable +#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE= +#define STATUS_REPORT_MIN_MS 100 // milliseconds - enforces a viable minimum +#define STATUS_REPORT_INTERVAL_MS 500 // milliseconds - set $SV=0 to disable #define STATUS_REPORT_DEFAULTS "posx","posy","posz","posa","feed","vel","unit","coor","dist","frmo","stat" -//tgfx-friendly defaults -//#define STATUS_REPORT_DEFAULTS "line","vel","mpox","mpoy","mpoz","mpoa","coor","ofsa","ofsx","ofsy","ofsz","dist","unit","stat","homz","homy","homx","momo" -#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE +#define QUEUE_REPORT_VERBOSITY QR_OFF // one of: QR_OFF, QR_SINGLE, QR_TRIPLE // Gcode startup defaults -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS -#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE +#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#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 // Comm mode and echo levels -#define COM_EXPAND_CR false -#define COM_ENABLE_ECHO false -#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON // FLOW_CONTROL_OFF, FLOW_CONTROL_XON, FLOW_CONTROL_RTS +#define COM_EXPAND_CR true +#define COM_ENABLE_ECHO true +#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON // FLOW_CONTROL_OFF, FLOW_CONTROL_XON, FLOW_CONTROL_RTS //**** DEBUG SETTINGS **** #ifdef __DEBUG_SETTINGS #undef QUEUE_REPORT_VERBOSITY -#define QUEUE_REPORT_VERBOSITY QR_SINGLE // one of: QR_OFF, QR_SINGLE, QR_TRIPLE +#define QUEUE_REPORT_VERBOSITY QR_SINGLE // one of: QR_OFF, QR_SINGLE, QR_TRIPLE #undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE #undef STATUS_REPORT_DEFAULTS #define STATUS_REPORT_DEFAULTS "posx","posy","posz","posa","feed","vel","unit","coor","dist","frmo","stat" -//#define STATUS_REPORT_DEFAULTS "line","posx","posy","posz","vel","_cs1","_es1","_fe1","_xs1","_cs2","_es2","_fe2","_xs2" #undef STATUS_REPORT_VERBOSITY -#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE #endif /**** MACHINE PROFILES ******************************************************/ // machine default profiles - choose only one: - -#include "settings/settings_default.h" // Default settings for release -//#include "settings/settings_cnc3040.h" -//#include "settings/settings_test.h" // Settings for testing - not for release -//#include "settings/settings_openpnp.h" // OpenPnP -//#include "settings/settings_othermill.h" // OMC OtherMill -//#include "settings/settings_probotixV90.h" // Probotix Fireball V90 -//#include "settings/settings_shapeoko2.h" // Shapeoko2 - standard kit -//#include "settings/settings_ultimaker.h" // Ultimaker 3D printer -//#include "settings/settings_zen7x12.h" // Zen Toolworks 7x12 +#include "settings/settings_default.h" // Default settings for release /*** Handle optional modules that may not be in every machine ***/ // If PWM_1 is not defined fill it with default values -#ifndef P1_PWM_FREQUENCY - -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 2000 -#define P1_CW_PHASE_LO 0.125 // phase [0..1] -#define P1_CW_PHASE_HI 0.2 -#define P1_CCW_SPEED_LO 1000 -#define P1_CCW_SPEED_HI 2000 -#define P1_CCW_PHASE_LO 0.125 -#define P1_CCW_PHASE_HI 0.2 -#define P1_PWM_PHASE_OFF 0.1 -#endif //P1_PWM_FREQUENCY +#ifndef P1_PWM_FREQUENCY +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 2000 +#define P1_CW_PHASE_LO 0.125 // phase [0..1] +#define P1_CW_PHASE_HI 0.2 +#define P1_CCW_SPEED_LO 1000 +#define P1_CCW_SPEED_HI 2000 +#define P1_CCW_PHASE_LO 0.125 +#define P1_CCW_PHASE_HI 0.2 +#define P1_PWM_PHASE_OFF 0.1 +#endif // P1_PWM_FREQUENCY /*** User-Defined Data Defaults ***/ -#define USER_DATA_A0 0 -#define USER_DATA_A1 0 -#define USER_DATA_A2 0 -#define USER_DATA_A3 0 -#define USER_DATA_B0 0 -#define USER_DATA_B1 0 -#define USER_DATA_B2 0 -#define USER_DATA_B3 0 -#define USER_DATA_C0 0 -#define USER_DATA_C1 0 -#define USER_DATA_C2 0 -#define USER_DATA_C3 0 -#define USER_DATA_D0 0 -#define USER_DATA_D1 0 -#define USER_DATA_D2 0 -#define USER_DATA_D3 0 +#define USER_DATA_A0 0 +#define USER_DATA_A1 0 +#define USER_DATA_A2 0 +#define USER_DATA_A3 0 +#define USER_DATA_B0 0 +#define USER_DATA_B1 0 +#define USER_DATA_B2 0 +#define USER_DATA_B3 0 +#define USER_DATA_C0 0 +#define USER_DATA_C1 0 +#define USER_DATA_C2 0 +#define USER_DATA_C3 0 +#define USER_DATA_D0 0 +#define USER_DATA_D1 0 +#define USER_DATA_D2 0 +#define USER_DATA_D3 0 #endif // End of include guard: SETTINGS_H_ONCE diff --git a/src/settings/settings_Ultimaker.h b/src/settings/settings_Ultimaker.h index fe8aa9a..7604c73 100755 --- a/src/settings/settings_Ultimaker.h +++ b/src/settings/settings_Ultimaker.h @@ -26,12 +26,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in hardware.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ @@ -41,8 +41,8 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to Ultimaker profile" -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 400000 // centripetal acceleration around corners +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 400000 // centripetal acceleration around corners #ifndef PI #define PI 3.14159628 @@ -51,165 +51,165 @@ // *** settings.h overrides *** #undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED +#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 40.64 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 1 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 40.64 -//#define M2_MICROSTEPS 8 -#define M2_MICROSTEPS 4 // correction for v9d boards -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 3.00 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE -#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 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWERED_IN_CYCLE -#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 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 40.64 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 1 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp + +#define M2_MOTOR_MAP AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 40.64 +//#define M2_MICROSTEPS 8 +#define M2_MICROSTEPS 4 // correction for v9d boards +#define M2_POLARITY 0 +#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 3.00 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 1 +#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M4_MOTOR_MAP AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 8 +#define M4_POLARITY 0 +#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE +#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 // degrees moved per motor rev +#define M5_MICROSTEPS 8 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_POWERED_IN_CYCLE +#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 // degrees moved per motor rev +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 20000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 212 // xtm travel between switches or crashes -#define X_JERK_MAX 40000 // xjm yes, that's "100 billion" mm/(min^3) -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_HOMING_LIMIT, SW_MODE_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_LIMIT // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_HOMING_LIMIT, SW_MODE_LIMIT -#define X_SEARCH_VELOCITY 3000 // xsv move in negative direction -#define X_LATCH_VELOCITY 200 // xlv mm/min -#define X_LATCH_BACKOFF 10 // xlb mm -#define X_ZERO_BACKOFF 3 // xzb mm -#define X_JERK_HOMING X_JERK_MAX // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 20000 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 190 -#define Y_JERK_MAX 40000 -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT -#define Y_SEARCH_VELOCITY 3000 -#define Y_LATCH_VELOCITY 200 -#define Y_LATCH_BACKOFF 10 -#define Y_ZERO_BACKOFF 3 -#define Y_JERK_HOMING Y_JERK_MAX - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 1000 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 220 -#define Z_JERK_MAX 50 // 50,000,000 -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 20000 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing +#define X_TRAVEL_MAX 212 // xtm travel between switches or crashes +#define X_JERK_MAX 40000 // xjm yes, that's "100 billion" mm/(min^3) +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_HOMING_LIMIT, SW_MODE_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_LIMIT // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_HOMING_LIMIT, SW_MODE_LIMIT +#define X_SEARCH_VELOCITY 3000 // xsv move in negative direction +#define X_LATCH_VELOCITY 200 // xlv mm/min +#define X_LATCH_BACKOFF 10 // xlb mm +#define X_ZERO_BACKOFF 3 // xzb mm +#define X_JERK_HOMING X_JERK_MAX // xjh + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX 20000 +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 190 +#define Y_JERK_MAX 40000 +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT +#define Y_SEARCH_VELOCITY 3000 +#define Y_LATCH_VELOCITY 200 +#define Y_LATCH_BACKOFF 10 +#define Y_ZERO_BACKOFF 3 +#define Y_JERK_HOMING Y_JERK_MAX + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX 1000 +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN 0 +#define Z_TRAVEL_MAX 220 +#define Z_JERK_MAX 50 // 50,000,000 +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED #define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 500 -#define Z_LATCH_VELOCITY 200 -#define Z_LATCH_BACKOFF 3 -#define Z_ZERO_BACKOFF 0.04 -#define Z_JERK_HOMING Z_JERK_MAX - -#define A_AXIS_MODE AXIS_RADIUS -#define A_RADIUS 0.609 +#define Z_SEARCH_VELOCITY 500 +#define Z_LATCH_VELOCITY 200 +#define Z_LATCH_BACKOFF 3 +#define Z_ZERO_BACKOFF 0.04 +#define Z_JERK_HOMING Z_JERK_MAX + +#define A_AXIS_MODE AXIS_RADIUS +#define A_RADIUS 0.609 #define A_VELOCITY_MAX 200000.000 -#define A_FEEDRATE_MAX 50000.000 -#define A_TRAVEL_MIN 0 -#define A_TRAVEL_MAX 10 -#define A_JERK_MAX 25000 -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_SWITCH_MODE_MIN SW_MODE_DISABLED -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 2000 -#define A_LATCH_VELOCITY 2000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_RADIUS -#define B_RADIUS 0.609 -#define B_VELOCITY_MAX 200000 -#define B_FEEDRATE_MAX 50000 -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX 25000 -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_SWITCH_MODE_MIN SW_MODE_DISABLED -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 2000 -#define B_LATCH_VELOCITY 2000 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN 0 -#define C_TRAVEL_MAX -1 -//#define C_JERK_MAX 20000000 -#define C_JERK_MAX 20 -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 -#define C_SWITCH_MODE_MIN SW_MODE_DISABLED -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 10 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX +#define A_FEEDRATE_MAX 50000.000 +#define A_TRAVEL_MIN 0 +#define A_TRAVEL_MAX 10 +#define A_JERK_MAX 25000 +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_SWITCH_MODE_MIN SW_MODE_DISABLED +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 2000 +#define A_LATCH_VELOCITY 2000 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX + +#define B_AXIS_MODE AXIS_RADIUS +#define B_RADIUS 0.609 +#define B_VELOCITY_MAX 200000 +#define B_FEEDRATE_MAX 50000 +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX 25000 +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_SWITCH_MODE_MIN SW_MODE_DISABLED +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SEARCH_VELOCITY 2000 +#define B_LATCH_VELOCITY 2000 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 +#define B_JERK_HOMING B_JERK_MAX + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX 3600 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN 0 +#define C_TRAVEL_MAX -1 +//#define C_JERK_MAX 20000000 +#define C_JERK_MAX 20 +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS 1 +#define C_SWITCH_MODE_MIN SW_MODE_DISABLED +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 10 +#define C_ZERO_BACKOFF 2 +#define C_JERK_HOMING C_JERK_MAX // *** DEFAULT COORDINATE SYSTEM OFFSETS *** -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table +#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table #define G55_Y_OFFSET (Y_TRAVEL_MAX/2) #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 diff --git a/src/settings/settings_cnc3040.h b/src/settings/settings_cnc3040.h index 79c468c..fb72234 100755 --- a/src/settings/settings_cnc3040.h +++ b/src/settings/settings_cnc3040.h @@ -25,12 +25,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in hardware.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ /**** Default profile for screw driven machines ************************/ @@ -41,170 +41,170 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to CNC3020 generic settings" -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners // **** settings.h overrides **** #undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED // to accommodate teh eStop switch / Amin +#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED // to accommodate teh eStop switch / Amin // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 4.02 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 4.02 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 4.02 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#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 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#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 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 4.02 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 0 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp + +#define M2_MOTOR_MAP AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 4.02 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 1 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 4.02 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 1 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M4_MOTOR_MAP AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 8 +#define M4_POLARITY 0 +#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 // degrees moved per motor rev +#define M5_MICROSTEPS 8 +#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 // degrees moved per motor rev +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWER_MODE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 5000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX 2500 // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 285 // xtm maximum travel - used by soft limits and homing -#define X_JERK_MAX 100 // xjm -#define X_JERK_HOMING X_JERK_MAX // xjh -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 500 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 1 // xzb mm - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 2000 -#define Y_FEEDRATE_MAX 2000 -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 390 -#define Y_JERK_MAX 100 -#define Y_JERK_HOMING Y_JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 500 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 1 - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 5000 -#define Z_FEEDRATE_MAX 2500 -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 55 -#define Z_JERK_MAX 100 -#define Z_JERK_HOMING Z_JERK_MAX -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 4007 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 1 +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 5000 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX 2500 // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing +#define X_TRAVEL_MAX 285 // xtm maximum travel - used by soft limits and homing +#define X_JERK_MAX 100 // xjm +#define X_JERK_HOMING X_JERK_MAX // xjh +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY 500 // xsv move in negative direction +#define X_LATCH_VELOCITY 100 // xlv mm/min +#define X_LATCH_BACKOFF 5 // xlb mm +#define X_ZERO_BACKOFF 1 // xzb mm + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX 2000 +#define Y_FEEDRATE_MAX 2000 +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 390 +#define Y_JERK_MAX 100 +#define Y_JERK_HOMING Y_JERK_MAX +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Y_SEARCH_VELOCITY 500 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 5 +#define Y_ZERO_BACKOFF 1 + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX 5000 +#define Z_FEEDRATE_MAX 2500 +#define Z_TRAVEL_MIN 0 +#define Z_TRAVEL_MAX 55 +#define Z_JERK_MAX 100 +#define Z_JERK_HOMING Z_JERK_MAX +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY 4007 +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 5 +#define Z_ZERO_BACKOFF 1 // Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_LIMIT -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED // DISABLED -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_DISABLED // DISABLED -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX +#define A_AXIS_MODE AXIS_RADIUS +#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit +#define A_TRAVEL_MAX -1 +#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_SWITCH_MODE_MIN SW_MODE_LIMIT +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX + +#define B_AXIS_MODE AXIS_DISABLED // DISABLED +#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define B_SWITCH_MODE_MIN SW_MODE_HOMING +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 +#define B_JERK_HOMING B_JERK_MAX + +#define C_AXIS_MODE AXIS_DISABLED // DISABLED +#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define C_SWITCH_MODE_MIN SW_MODE_HOMING +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 +#define C_JERK_HOMING C_JERK_MAX // *** DEFAULT COORDINATE SYSTEM OFFSETS *** -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table +#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table #define G55_Y_OFFSET (Y_TRAVEL_MAX/2) #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 diff --git a/src/settings/settings_default.h b/src/settings/settings_default.h index 36cb5cf..ac94f30 100755 --- a/src/settings/settings_default.h +++ b/src/settings/settings_default.h @@ -25,12 +25,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in hardware.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ /**** Default profile for screw driven machines ************************/ @@ -41,168 +41,168 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to default settings" -#define JERK_MAX 20 // yes, that's "20,000,000" mm/(min^3) -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners +#define JERK_MAX 20 // yes, that's "20,000,000" mm/(min^3) +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners // **** settings.h overrides **** // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 1.25 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 1.25 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 1.25 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#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 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#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 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 1.25 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 0 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp + +#define M2_MOTOR_MAP AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 1.25 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 0 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 1.25 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M4_MOTOR_MAP AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 8 +#define M4_POLARITY 0 +#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 // degrees moved per motor rev +#define M5_MICROSTEPS 8 +#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 // degrees moved per motor rev +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWER_MODE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 800 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn monimum travel for soft limits -#define X_TRAVEL_MAX 150 // xtm travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JERK_HOMING (X_JERK_MAX * 2) // xjh -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 500 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 1 // xzb mm - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 800 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 150 -#define Y_JERK_MAX JERK_MAX -#define Y_JERK_HOMING (Y_JERK_MAX * 2) -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 500 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 1 - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 800 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 75 -#define Z_JERK_MAX JERK_MAX -#define Z_JERK_HOMING (Z_JERK_MAX * 2) -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 400 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 1 +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 800 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn monimum travel for soft limits +#define X_TRAVEL_MAX 150 // xtm travel between switches or crashes +#define X_JERK_MAX JERK_MAX // xjm +#define X_JERK_HOMING (X_JERK_MAX * 2) // xjh +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY 500 // xsv move in negative direction +#define X_LATCH_VELOCITY 100 // xlv mm/min +#define X_LATCH_BACKOFF 5 // xlb mm +#define X_ZERO_BACKOFF 1 // xzb mm + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX 800 +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 150 +#define Y_JERK_MAX JERK_MAX +#define Y_JERK_HOMING (Y_JERK_MAX * 2) +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Y_SEARCH_VELOCITY 500 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 5 +#define Y_ZERO_BACKOFF 1 + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX 800 +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN 0 +#define Z_TRAVEL_MAX 75 +#define Z_JERK_MAX JERK_MAX +#define Z_JERK_HOMING (Z_JERK_MAX * 2) +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY 400 +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 5 +#define Z_ZERO_BACKOFF 1 // A values are chosen to make the A motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 -#define A_TRAVEL_MAX -1 // same number means infinite -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JERK_HOMING (A_JERK_MAX * 2) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX JERK_MAX -#define B_JERK_HOMING (B_JERK_MAX * 2) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX JERK_MAX -#define C_JERK_HOMING (C_JERK_MAX * 2) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 +#define A_AXIS_MODE AXIS_RADIUS +#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 +#define A_TRAVEL_MAX -1 // same number means infinite +#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define A_JERK_HOMING (A_JERK_MAX * 2) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 + +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX 3600 +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX JERK_MAX +#define B_JERK_HOMING (B_JERK_MAX * 2) +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS 1 +#define B_SWITCH_MODE_MIN SW_MODE_HOMING +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX 3600 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX JERK_MAX +#define C_JERK_HOMING (C_JERK_MAX * 2) +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS 1 +#define C_SWITCH_MODE_MIN SW_MODE_HOMING +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 // *** DEFAULT COORDINATE SYSTEM OFFSETS *** -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table +#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table #define G55_Y_OFFSET (Y_TRAVEL_MAX/2) #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 diff --git a/src/settings/settings_openpnp.h b/src/settings/settings_openpnp.h index 271e251..c312cc6 100755 --- a/src/settings/settings_openpnp.h +++ b/src/settings/settings_openpnp.h @@ -25,12 +25,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in system.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in system.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ @@ -40,160 +40,160 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to OpenPnP experimental profile" -#define JUNCTION_DEVIATION 0.01 // default value, in mm - smaller is faster -#define JUNCTION_ACCELERATION 2000000 // 2 million - centripetal acceleration around corners +#define JUNCTION_DEVIATION 0.01 // default value, in mm - smaller is faster +#define JUNCTION_ACCELERATION 2000000 // 2 million - centripetal acceleration around corners // *** settings.h overrides *** #undef COMM_MODE -#define COMM_MODE TEXT_MODE +#define COMM_MODE TEXT_MODE #undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_LINENUM +#define JSON_VERBOSITY JV_LINENUM #undef COM_ENABLE_XON -#define COM_ENABLE_XON true +#define COM_ENABLE_XON true // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 25.4 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 36.54 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 1.25 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 180 // degrees per motor rev - 1:2 gearing -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE - -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 25.4 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 0 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm + +#define M2_MOTOR_MAP AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 36.54 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 0 +#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE + +#define M3_MOTOR_MAP AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 1.25 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE + +#define M4_MOTOR_MAP AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 180 // degrees per motor rev - 1:2 gearing +#define M4_MICROSTEPS 8 +#define M4_POLARITY 0 +#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE + +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 15000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MAX 220 // xtm travel between switches or crashes -#define X_TRAVEL_MIN 0 // xtn monimum travel for soft limits -#define X_JERK_MAX 2500 // xjm 2.5 billion mm/(min^3) -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 3000 // xsv minus means move to minimum switch -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 20 // xlb mm -#define X_ZERO_BACKOFF 3 // xzb mm -#define X_JERK_HOMING 10000 // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 16000 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MAX 220 -#define Y_TRAVEL_MIN 0 -#define Y_JERK_MAX 5000 // 5,000,000,000 -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 3000 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 20 -#define Y_ZERO_BACKOFF 3 -#define Y_JERK_HOMING 10000 // xjh - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 800 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MAX 100 -#define Z_TRAVEL_MIN 0 -#define Z_JERK_MAX 50 // 50,000,000 -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY Z_VELOCITY_MAX -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 20 -#define Z_ZERO_BACKOFF 10 -#define Z_JERK_HOMING 1000 // xjh - -#define A_AXIS_MODE AXIS_STANDARD -#define A_VELOCITY_MAX 60000 -#define A_FEEDRATE_MAX 48000 -#define A_TRAVEL_MAX 400 // degrees -#define A_TRAVEL_MIN -1 // -1 means infinite, no limit -#define A_JERK_MAX 24000 // yes, 24 billion -#define A_JUNCTION_DEVIATION 0.1 -#define A_RADIUS 1.0 -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 6000 -#define A_LATCH_VELOCITY 1000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX -1 -#define B_TRAVEL_MIN -1 -#define B_JERK_MAX 20 -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX -1 -#define C_TRAVEL_MIN -1 -#define C_JERK_MAX 20 -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 15000 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MAX 220 // xtm travel between switches or crashes +#define X_TRAVEL_MIN 0 // xtn monimum travel for soft limits +#define X_JERK_MAX 2500 // xjm 2.5 billion mm/(min^3) +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY 3000 // xsv minus means move to minimum switch +#define X_LATCH_VELOCITY 100 // xlv mm/min +#define X_LATCH_BACKOFF 20 // xlb mm +#define X_ZERO_BACKOFF 3 // xzb mm +#define X_JERK_HOMING 10000 // xjh + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX 16000 +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MAX 220 +#define Y_TRAVEL_MIN 0 +#define Y_JERK_MAX 5000 // 5,000,000,000 +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Y_SEARCH_VELOCITY 3000 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 20 +#define Y_ZERO_BACKOFF 3 +#define Y_JERK_HOMING 10000 // xjh + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX 800 +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MAX 100 +#define Z_TRAVEL_MIN 0 +#define Z_JERK_MAX 50 // 50,000,000 +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY Z_VELOCITY_MAX +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 20 +#define Z_ZERO_BACKOFF 10 +#define Z_JERK_HOMING 1000 // xjh + +#define A_AXIS_MODE AXIS_STANDARD +#define A_VELOCITY_MAX 60000 +#define A_FEEDRATE_MAX 48000 +#define A_TRAVEL_MAX 400 // degrees +#define A_TRAVEL_MIN -1 // -1 means infinite, no limit +#define A_JERK_MAX 24000 // yes, 24 billion +#define A_JUNCTION_DEVIATION 0.1 +#define A_RADIUS 1.0 +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 6000 +#define A_LATCH_VELOCITY 1000 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX + +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX 3600 +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MAX -1 +#define B_TRAVEL_MIN -1 +#define B_JERK_MAX 20 +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS 1 + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX 3600 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MAX -1 +#define C_TRAVEL_MIN -1 +#define C_JERK_MAX 20 +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS 1 // *** DEFAULT COORDINATE SYSTEM OFFSETS *** // Our convention is: -// - leave G54 in machine coordinates to act as a persistent absolute coordinate system -// - set G55 to be a zero in the middle of the table -// - no action for the others +// - leave G54 in machine coordinates to act as a persistent absolute coordinate system +// - set G55 to be a zero in the middle of the table +// - no action for the others -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set g55 to middle of table +#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set g55 to middle of table #define G55_Y_OFFSET (Y_TRAVEL_MAX/2) #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 #define G55_B_OFFSET 0 #define G55_C_OFFSET 0 -#define G56_X_OFFSET (X_TRAVEL_MAX/2) // special settings for running braid tests +#define G56_X_OFFSET (X_TRAVEL_MAX/2) // special settings for running braid tests #define G56_Y_OFFSET 20 #define G56_Z_OFFSET -10 #define G56_A_OFFSET 0 diff --git a/src/settings/settings_othermill.h b/src/settings/settings_othermill.h index ee59f70..a6f5c39 100755 --- a/src/settings/settings_othermill.h +++ b/src/settings/settings_othermill.h @@ -23,12 +23,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in hardware.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ @@ -38,11 +38,11 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to OMC OtherMill settings" -#define JERK_MAX 500 // 500 million mm/(min^3) -#define JERK_HOMING 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* -#define JUNCTION_DEVIATION 0.01 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners -#define LATCH_VELOCITY 25 // reeeeally slow for accuracy +#define JERK_MAX 500 // 500 million mm/(min^3) +#define JERK_HOMING 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* +#define JUNCTION_DEVIATION 0.01 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners +#define LATCH_VELOCITY 25 // reeeeally slow for accuracy // WARNING: Older Othermill machines use a 15deg can stack for their Z axis. // new machines use a stepper which has the same config as the other axis. @@ -57,29 +57,29 @@ #undef STATUS_REPORT_DEFAULTS #define STATUS_REPORT_DEFAULTS "mpox","mpoy","mpoz","mpoa","ofsx","ofsy","ofsz","ofsa","unit","stat","coor","momo","dist","home","hold","macs","cycs","mots","plan","prbe" -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED +#undef SWITCH_TYPE +#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED -#undef COMM_MODE -#define COMM_MODE JSON_MODE +#undef COMM_MODE +#define COMM_MODE JSON_MODE -#undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#undef JSON_VERBOSITY +#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE #undef JSON_FOOTER_DEPTH -#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style +#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style #undef JSON_SYNTAX_MODE -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT +#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT -#undef QUEUE_REPORT_VERBOSITY -#define QUEUE_REPORT_VERBOSITY QR_SINGLE +#undef QUEUE_REPORT_VERBOSITY +#define QUEUE_REPORT_VERBOSITY QR_SINGLE -#undef STATUS_REPORT_VERBOSITY -#define STATUS_REPORT_VERBOSITY SR_FILTERED +#undef STATUS_REPORT_VERBOSITY +#define STATUS_REPORT_VERBOSITY SR_FILTERED #undef COM_ENABLE_FLOW_CONTROL -#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON +#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON #undef GCODE_DEFAULT_COORD_SYSTEM #undef GCODE_DEFAULT_UNITS @@ -88,177 +88,177 @@ #undef GCODE_DEFAULT_PATH_CONTROL #undef GCODE_DEFAULT_DISTANCE_MODE -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS +#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 +#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE // *** motor settings *** -#define M4_MOTOR_MAP AXIS_X // 1ma -#define M4_STEP_ANGLE 1.8 // 1sa -#define M4_TRAVEL_PER_REV 5.08 // 1tr -#define M4_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M4_POLARITY 0 // 1po 0=normal, 1=reversed -#define M4_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL // v9 only - -#define M3_MOTOR_MAP AXIS_Y -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 5.08 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M2_MOTOR_MAP AXIS_Z +#define M4_MOTOR_MAP AXIS_X // 1ma +#define M4_STEP_ANGLE 1.8 // 1sa +#define M4_TRAVEL_PER_REV 5.08 // 1tr +#define M4_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M4_POLARITY 0 // 1po 0=normal, 1=reversed +#define M4_POWER_MODE MOTOR_POWER_MODE // 1pm TRUE=low power idle enabled +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL // v9 only + +#define M3_MOTOR_MAP AXIS_Y +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 5.08 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 1 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M2_MOTOR_MAP AXIS_Z #if HAS_CANSTACK_Z_AXIS -#define M2_STEP_ANGLE 15 -#define M2_TRAVEL_PER_REV 1.27254 +#define M2_STEP_ANGLE 15 +#define M2_TRAVEL_PER_REV 1.27254 #else -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 5.08 +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 5.08 #endif -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M1_MOTOR_MAP AXIS_A -#define M1_STEP_ANGLE 1.8 -#define M1_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M1_MICROSTEPS 8 -#define M1_POLARITY 1 -#define M1_POWER_MODE MOTOR_POWER_MODE -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M2_MICROSTEPS 8 +#define M2_POLARITY 1 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M1_MOTOR_MAP AXIS_A +#define M1_STEP_ANGLE 1.8 +#define M1_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M1_MICROSTEPS 8 +#define M1_POLARITY 1 +#define M1_POWER_MODE MOTOR_POWER_MODE +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 138 // xtr travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX/3) // xsv -#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 0 // xzb mm -#define X_JERK_HOMING JERK_HOMING // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX X_VELOCITY_MAX -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 115 -#define Y_JERK_MAX JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX/3) -#define Y_LATCH_VELOCITY LATCH_VELOCITY -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 3 -#define Y_JERK_HOMING JERK_HOMING - -#define Z_AXIS_MODE AXIS_STANDARD +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX 138 // xtr travel between switches or crashes +#define X_JERK_MAX JERK_MAX // xjm +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX/3) // xsv +#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min +#define X_LATCH_BACKOFF 5 // xlb mm +#define X_ZERO_BACKOFF 0 // xzb mm +#define X_JERK_HOMING JERK_HOMING // xjh + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX X_VELOCITY_MAX +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 115 +#define Y_JERK_MAX JERK_MAX +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX/3) +#define Y_LATCH_VELOCITY LATCH_VELOCITY +#define Y_LATCH_BACKOFF 5 +#define Y_ZERO_BACKOFF 3 +#define Y_JERK_HOMING JERK_HOMING + +#define Z_AXIS_MODE AXIS_STANDARD #if HAS_CANSTACK_Z_AXIS -#define Z_VELOCITY_MAX 1000 +#define Z_VELOCITY_MAX 1000 #else -#define Z_VELOCITY_MAX X_VELOCITY_MAX +#define Z_VELOCITY_MAX X_VELOCITY_MAX #endif -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN -70 -#define Z_TRAVEL_MAX 0 -#define Z_JERK_MAX JERK_MAX // 200 million -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX/3) -#define Z_LATCH_VELOCITY LATCH_VELOCITY -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 0 -#define Z_JERK_HOMING JERK_HOMING +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN -70 +#define Z_TRAVEL_MAX 0 +#define Z_JERK_MAX JERK_MAX // 200 million +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX/3) +#define Z_LATCH_VELOCITY LATCH_VELOCITY +#define Z_LATCH_BACKOFF 5 +#define Z_ZERO_BACKOFF 0 +#define Z_JERK_HOMING JERK_HOMING // Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED // DISABLED -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_DISABLED // DISABLED -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX +#define A_AXIS_MODE AXIS_RADIUS +#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit +#define A_TRAVEL_MAX -1 +#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX + +#define B_AXIS_MODE AXIS_DISABLED // DISABLED +#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define B_SWITCH_MODE_MIN SW_MODE_HOMING +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 +#define B_JERK_HOMING B_JERK_MAX + +#define C_AXIS_MODE AXIS_DISABLED // DISABLED +#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define C_SWITCH_MODE_MIN SW_MODE_HOMING +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 +#define C_JERK_HOMING C_JERK_MAX // *** PWM SPINDLE CONTROL *** -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 7900 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 12800 -#define P1_CW_PHASE_LO 0.13 // phase [0..1] -#define P1_CW_PHASE_HI 0.17 -#define P1_CCW_SPEED_LO 0 -#define P1_CCW_SPEED_HI 0 -#define P1_CCW_PHASE_LO 0.1 -#define P1_CCW_PHASE_HI 0.1 -#define P1_PWM_PHASE_OFF 0.1 +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 7900 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 12800 +#define P1_CW_PHASE_LO 0.13 // phase [0..1] +#define P1_CW_PHASE_HI 0.17 +#define P1_CCW_SPEED_LO 0 +#define P1_CCW_SPEED_HI 0 +#define P1_CCW_PHASE_LO 0.1 +#define P1_CCW_PHASE_HI 0.1 +#define P1_PWM_PHASE_OFF 0.1 // *** DEFAULT COORDINATE SYSTEM OFFSETS *** -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET 0 // but the again, so is everyting else (at least for start) +#define G55_X_OFFSET 0 // but the again, so is everyting else (at least for start) #define G55_Y_OFFSET 0 #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 diff --git a/src/settings/settings_probotixV90.h b/src/settings/settings_probotixV90.h index c71a236..7a415c6 100755 --- a/src/settings/settings_probotixV90.h +++ b/src/settings/settings_probotixV90.h @@ -25,12 +25,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in hardware.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ @@ -40,161 +40,161 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to Probotix Fireball V90 profile" -#define JERK_MAX 100 // yes, that's "100,000,000" mm/(min^3) -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 200000 // centripetal acceleration around corners +#define JERK_MAX 100 // yes, that's "100,000,000" mm/(min^3) +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 200000 // centripetal acceleration around corners // **** settings.h overrides **** // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 5.08 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 5.08 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWER_MODE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 2.1166666 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE MOTOR_POWER_MODE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#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 // degrees moved per motor rev -#define M5_MICROSTEPS 8 -#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 // degrees moved per motor rev -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 5.08 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 0 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1pl + +#define M2_MOTOR_MAP AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 5.08 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 0 +#define M2_POWER_MODE MOTOR_POWER_MODE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 2.1166666 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 0 +#define M3_POWER_MODE MOTOR_POWER_MODE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M4_MOTOR_MAP AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 8 +#define M4_POLARITY 0 +#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 // degrees moved per motor rev +#define M5_MICROSTEPS 8 +#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 // degrees moved per motor rev +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWER_MODE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 2400 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 400 // xtm maximum travel - used by soft limits and homing -#define X_JERK_MAX JERK_MAX // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 1000 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 10 // xlb mm -#define X_ZERO_BACKOFF 2 // xzb mm -#define X_JERK_HOMING X_JERK_MAX // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 2400 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 175 -#define Y_JERK_MAX JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT -#define Y_SEARCH_VELOCITY 1000 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 10 -#define Y_ZERO_BACKOFF 2 -#define Y_JERK_HOMING Y_JERK_MAX - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 1200 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 75 -#define Z_JERK_MAX JERK_MAX -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 600 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 10 -#define Z_ZERO_BACKOFF 2 -#define Z_JERK_HOMING Z_JERK_MAX +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 2400 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing +#define X_TRAVEL_MAX 400 // xtm maximum travel - used by soft limits and homing +#define X_JERK_MAX JERK_MAX // xjm +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY 1000 // xsv move in negative direction +#define X_LATCH_VELOCITY 100 // xlv mm/min +#define X_LATCH_BACKOFF 10 // xlb mm +#define X_ZERO_BACKOFF 2 // xzb mm +#define X_JERK_HOMING X_JERK_MAX // xjh + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX 2400 +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 175 +#define Y_JERK_MAX JERK_MAX +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT +#define Y_SEARCH_VELOCITY 1000 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 10 +#define Y_ZERO_BACKOFF 2 +#define Y_JERK_HOMING Y_JERK_MAX + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX 1200 +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN 0 +#define Z_TRAVEL_MAX 75 +#define Z_JERK_MAX JERK_MAX +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY 600 +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 10 +#define Z_ZERO_BACKOFF 2 +#define Z_JERK_HOMING Z_JERK_MAX // Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_RADIUS -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_RADIUS -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX +#define A_AXIS_MODE AXIS_RADIUS +#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit +#define A_TRAVEL_MAX -1 +#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX + +#define B_AXIS_MODE AXIS_RADIUS +#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define B_SWITCH_MODE_MIN SW_MODE_HOMING +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 +#define B_JERK_HOMING B_JERK_MAX + +#define C_AXIS_MODE AXIS_RADIUS +#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define C_SWITCH_MODE_MIN SW_MODE_HOMING +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 +#define C_JERK_HOMING C_JERK_MAX // *** DEFAULT COORDINATE SYSTEM OFFSETS *** -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 diff --git a/src/settings/settings_shapeoko2.h b/src/settings/settings_shapeoko2.h index d1b0c34..a3c6c7d 100755 --- a/src/settings/settings_shapeoko2.h +++ b/src/settings/settings_shapeoko2.h @@ -25,12 +25,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in system.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in system.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ @@ -40,113 +40,113 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to Shapeoko2 500mm profile" -#define JUNCTION_DEVIATION 0.01 // default value, in mm - smaller is faster -#define JUNCTION_ACCELERATION 2000000 // 2 million - centripetal acceleration around corners +#define JUNCTION_DEVIATION 0.01 // default value, in mm - smaller is faster +#define JUNCTION_ACCELERATION 2000000 // 2 million - centripetal acceleration around corners // *** settings.h overrides *** #undef COMM_MODE -#define COMM_MODE JSON_MODE +#define COMM_MODE JSON_MODE #undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_VERBOSE +#define JSON_VERBOSITY JV_VERBOSE #undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED +#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 40.00 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 1 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE 2 // 1pm TRUE=low power idle enabled - -#define M2_MOTOR_MAP AXIS_Y // Y1 - left side of machine -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 40.00 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE 2 - -#define M3_MOTOR_MAP AXIS_Y // Y2 - right sif of machine -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 40.00 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 0 -#define M3_POWER_MODE 2 - -#define M4_MOTOR_MAP AXIS_Z -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 2.1166 -#define M4_MICROSTEPS 8 -#define M4_POLARITY 1 -#define M4_POWER_MODE 2 - -#define M5_MOTOR_MAP AXIS_DISABLED -#define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees per motor rev -#define M5_MICROSTEPS 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWER_MODE - -#define M6_MOTOR_MAP AXIS_DISABLED -#define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 -#define M6_MICROSTEPS 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWER_MODE +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 40.00 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 1 // 1po 0=normal, 1=reversed +#define M1_POWER_MODE 2 // 1pm TRUE=low power idle enabled + +#define M2_MOTOR_MAP AXIS_Y // Y1 - left side of machine +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 40.00 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 1 +#define M2_POWER_MODE 2 + +#define M3_MOTOR_MAP AXIS_Y // Y2 - right sif of machine +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 40.00 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 0 +#define M3_POWER_MODE 2 + +#define M4_MOTOR_MAP AXIS_Z +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 2.1166 +#define M4_MICROSTEPS 8 +#define M4_POLARITY 1 +#define M4_POWER_MODE 2 + +#define M5_MOTOR_MAP AXIS_DISABLED +#define M5_STEP_ANGLE 1.8 +#define M5_TRAVEL_PER_REV 360 // degrees per motor rev +#define M5_MICROSTEPS 8 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_POWER_MODE + +#define M6_MOTOR_MAP AXIS_DISABLED +#define M6_STEP_ANGLE 1.8 +#define M6_TRAVEL_PER_REV 360 +#define M6_MICROSTEPS 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWER_MODE // *** axis settings *** // These are relative conservative values for a well-tuned Shapeoko2 or similar XY belt / Z screw machine -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 16000 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel -#define X_TRAVEL_MAX 290 // xtm maximum travel (travel between switches or crashes) -#define X_JERK_MAX 5000 // xjm yes, that's "5 billion" mm/(min^3) -#define X_JERK_HOMING 10000 // xjh -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 3000 // xsv minus means move to minimum switch -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 10 // xlb mm -#define X_ZERO_BACKOFF 2 // xzb mm - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 16000 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 320 -#define Y_JERK_MAX 5000 -#define Y_JERK_HOMING 10000 // xjh -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 3000 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 10 -#define Y_ZERO_BACKOFF 2 - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 1000 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MAX 0 -#define Z_TRAVEL_MIN -120 // this is approximate as Z depth depends on tooling +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 16000 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel +#define X_TRAVEL_MAX 290 // xtm maximum travel (travel between switches or crashes) +#define X_JERK_MAX 5000 // xjm yes, that's "5 billion" mm/(min^3) +#define X_JERK_HOMING 10000 // xjh +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY 3000 // xsv minus means move to minimum switch +#define X_LATCH_VELOCITY 100 // xlv mm/min +#define X_LATCH_BACKOFF 10 // xlb mm +#define X_ZERO_BACKOFF 2 // xzb mm + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX 16000 +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 320 +#define Y_JERK_MAX 5000 +#define Y_JERK_HOMING 10000 // xjh +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Y_SEARCH_VELOCITY 3000 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 10 +#define Y_ZERO_BACKOFF 2 + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX 1000 +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MAX 0 +#define Z_TRAVEL_MIN -120 // this is approximate as Z depth depends on tooling // value must be large enough to guarantee return to Zmax during homing -#define Z_JERK_MAX 50 // 50,000,000 -#define Z_JERK_HOMING 1000 -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY Z_VELOCITY_MAX -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 10 -#define Z_ZERO_BACKOFF 3 +#define Z_JERK_MAX 50 // 50,000,000 +#define Z_JERK_HOMING 1000 +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY Z_VELOCITY_MAX +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 10 +#define Z_ZERO_BACKOFF 3 /*************************************************************************************** * A Axis rotary values are chosen to make the motor react the same as X for testing @@ -167,85 +167,85 @@ * ***************************************************************************************/ -#define A_AXIS_MODE AXIS_RADIUS -#define A_RADIUS 5.30516 // +#define A_AXIS_MODE AXIS_RADIUS +#define A_RADIUS 5.30516 // #define A_VELOCITY_MAX 25920.0 // ~40 mm/s, 2,400 mm/min -#define A_FEEDRATE_MAX 25920.0/2.0 // ~20 mm/s, 1,200 mm/min -#define A_TRAVEL_MIN -1 // identical mean no homing will occur -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX 324000 // 1,000 million mm/min^3 +#define A_FEEDRATE_MAX 25920.0/2.0 // ~20 mm/s, 1,200 mm/min +#define A_TRAVEL_MIN -1 // identical mean no homing will occur +#define A_TRAVEL_MAX -1 +#define A_JERK_MAX 324000 // 1,000 million mm/min^3 // * a million IF it's over a million // c=2*pi*r, r=5.30516476972984, d=c/360, s=((1000*60)/d) -#define A_JERK_HOMING A_JERK_MAX -#define A_JUNCTION_DEVIATION 0.1 -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 2000 -#define A_LATCH_VELOCITY 2000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX +#define A_JUNCTION_DEVIATION 0.1 +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 2000 +#define A_LATCH_VELOCITY 2000 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 /* -#define A_AXIS_MODE AXIS_STANDARD -#define A_VELOCITY_MAX 60000 -#define A_FEEDRATE_MAX 48000 -#define A_JERK_MAX 24000 // yes, 24 billion -#define A_JERK_HOMING A_JERK_MAX -#define A_RADIUS 1.0 -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 6000 -#define A_LATCH_VELOCITY 1000 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 +#define A_AXIS_MODE AXIS_STANDARD +#define A_VELOCITY_MAX 60000 +#define A_FEEDRATE_MAX 48000 +#define A_JERK_MAX 24000 // yes, 24 billion +#define A_JERK_HOMING A_JERK_MAX +#define A_RADIUS 1.0 +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 6000 +#define A_LATCH_VELOCITY 1000 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 */ -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX -1 -#define B_TRAVEL_MIN -1 -#define B_JERK_MAX 20 -#define B_JERK_HOMING B_JERK_MAX -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 6000 -#define B_LATCH_VELOCITY 1000 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX -1 -#define C_TRAVEL_MIN -1 -#define C_JERK_MAX 20 -#define C_JERK_HOMING C_JERK_MAX -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 6000 -#define C_LATCH_VELOCITY 1000 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX 3600 +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MAX -1 +#define B_TRAVEL_MIN -1 +#define B_JERK_MAX 20 +#define B_JERK_HOMING B_JERK_MAX +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS 1 +#define B_SWITCH_MODE_MIN SW_MODE_HOMING +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SEARCH_VELOCITY 6000 +#define B_LATCH_VELOCITY 1000 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX 3600 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MAX -1 +#define C_TRAVEL_MIN -1 +#define C_JERK_MAX 20 +#define C_JERK_HOMING C_JERK_MAX +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS 1 +#define C_SWITCH_MODE_MIN SW_MODE_HOMING +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SEARCH_VELOCITY 6000 +#define C_LATCH_VELOCITY 1000 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 // *** DEFAULT COORDINATE SYSTEM OFFSETS *** // Our convention is: -// - leave G54 in machine coordinates to act as a persistent absolute coordinate system -// - set G55 to be a zero in the middle of the table -// - no action for the others +// - leave G54 in machine coordinates to act as a persistent absolute coordinate system +// - set G55 to be a zero in the middle of the table +// - no action for the others -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set g55 to middle of table +#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set g55 to middle of table #define G55_Y_OFFSET (Y_TRAVEL_MAX/2) #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 diff --git a/src/settings/settings_test.h b/src/settings/settings_test.h index 54429be..749b3b9 100755 --- a/src/settings/settings_test.h +++ b/src/settings/settings_test.h @@ -25,12 +25,12 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. * - * System and hardware settings that you shouldn't need to change - * are in hardware.h Application settings that also shouldn't need - * to be changed are in tinyg.h + * System and hardware settings that you shouldn't need to change + * are in hardware.h Application settings that also shouldn't need + * to be changed are in tinyg.h */ /***********************************************************************/ @@ -40,72 +40,72 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to TEST settings" -#define JERK_MAX 500 // 500 million mm/(min^3) -#define JERK_HOMING 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* -#define JUNCTION_DEVIATION 0.01 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners -#define LATCH_VELOCITY 25 // reeeeally slow for accuracy +#define JERK_MAX 500 // 500 million mm/(min^3) +#define JERK_HOMING 1000 // 1000 million mm/(min^3) // Jerk during homing needs to stop *fast* +#define JUNCTION_DEVIATION 0.01 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners +#define LATCH_VELOCITY 25 // reeeeally slow for accuracy // WARNING: Older Othermill machines use a 15deg can stack for their Z axis. // new machines use a stepper which has the same config as the other axis. -//#define HAS_CANSTACK_Z_AXIS 0 -#define HAS_CANSTACK_Z_AXIS 1 // Earlier machines +//#define HAS_CANSTACK_Z_AXIS 0 +#define HAS_CANSTACK_Z_AXIS 1 // Earlier machines // *** settings.h overrides *** // Note: there are some commented test values below #undef JSON_VERBOSITY -//#define JSON_VERBOSITY JV_SILENT // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -//#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define JSON_VERBOSITY JV_VERBOSE // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +//#define JSON_VERBOSITY JV_SILENT // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +//#define JSON_VERBOSITY JV_MESSAGES // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define JSON_VERBOSITY JV_VERBOSE // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE /* #undef JSON_SYNTAX_MODE -#define JSON_SYNTAX_MODE JSON_SYNTAX_RELAXED // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT +#define JSON_SYNTAX_MODE JSON_SYNTAX_RELAXED // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT +#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT // one of JSON_SYNTAX_RELAXED, JSON_SYNTAX_STRICT */ #undef STATUS_REPORT_VERBOSITY -//#define STATUS_REPORT_VERBOSITY SR_OFF // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -//#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE -#define STATUS_REPORT_VERBOSITY SR_VERBOSE // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +//#define STATUS_REPORT_VERBOSITY SR_OFF // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +//#define STATUS_REPORT_VERBOSITY SR_FILTERED // one of: SR_OFF, SR_FILTERED, SR_VERBOSE +#define STATUS_REPORT_VERBOSITY SR_VERBOSE // one of: SR_OFF, SR_FILTERED, SR_VERBOSE #undef STATUS_REPORT_DEFAULTS //#define STATUS_REPORT_DEFAULTS "mpox","mpoy","mpoz","mpoa","ofsx","ofsy","ofsz","ofsa","unit","stat","coor","momo","dist","home","hold","macs","cycs","mots","plan","feed" //#define STATUS_REPORT_DEFAULTS "line","mpox","mpoy","mpoz","mpoa","ofsx","ofsy","ofsz","ofsa","stat","_cs1","_es1","_fe0","_fe1","_fe2","_fe3" //#define STATUS_REPORT_DEFAULTS "line","mpox","mpoy","mpoz","stat","_ts2","_ps2","_cs2","_es2","_fe2" -#define STATUS_REPORT_DEFAULTS "line","mpox","mpoy","mpoz","_cs3","_es3","_fe3","_xs3","_cs2","_es2","_fe2","_xs2","stat" +#define STATUS_REPORT_DEFAULTS "line","mpox","mpoy","mpoz","_cs3","_es3","_fe3","_xs3","_cs2","_es2","_fe2","_xs2","stat" //#define STATUS_REPORT_DEFAULTS "line","posx","posy","posz","posa","feed","vel","unit","coor","dist","frmo","momo","stat","_cs1","_es1","_xs1","_fe1" -#undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED +#undef SWITCH_TYPE +#define SWITCH_TYPE SW_TYPE_NORMALLY_CLOSED -#undef COMM_MODE -#define COMM_MODE JSON_MODE +#undef COMM_MODE +#define COMM_MODE JSON_MODE -#undef JSON_VERBOSITY -//#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE -#define JSON_VERBOSITY JV_VERBOSE // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#undef JSON_VERBOSITY +//#define JSON_VERBOSITY JV_CONFIGS // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE +#define JSON_VERBOSITY JV_VERBOSE // one of: JV_SILENT, JV_FOOTER, JV_CONFIGS, JV_MESSAGES, JV_LINENUM, JV_VERBOSE #undef JSON_FOOTER_DEPTH -#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style +#define JSON_FOOTER_DEPTH 0 // 0 = new style, 1 = old style #undef JSON_SYNTAX_MODE -#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT +#define JSON_SYNTAX_MODE JSON_SYNTAX_STRICT -//#undef QUEUE_REPORT_VERBOSITY -//#define QUEUE_REPORT_VERBOSITY QR_SINGLE +//#undef QUEUE_REPORT_VERBOSITY +//#define QUEUE_REPORT_VERBOSITY QR_SINGLE -#undef STATUS_REPORT_VERBOSITY -//#define STATUS_REPORT_VERBOSITY SR_FILTERED -#define STATUS_REPORT_VERBOSITY SR_VERBOSE +#undef STATUS_REPORT_VERBOSITY +//#define STATUS_REPORT_VERBOSITY SR_FILTERED +#define STATUS_REPORT_VERBOSITY SR_VERBOSE #undef COM_ENABLE_FLOW_CONTROL -#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON +#define COM_ENABLE_FLOW_CONTROL FLOW_CONTROL_XON #undef GCODE_DEFAULT_COORD_SYSTEM #undef GCODE_DEFAULT_UNITS @@ -114,166 +114,166 @@ #undef GCODE_DEFAULT_PATH_CONTROL #undef GCODE_DEFAULT_DISTANCE_MODE -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS +#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#define GCODE_DEFAULT_COORD_SYSTEM G55 // G54, G55, G56, G57, G58 or G59 +#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE // *** motor settings *** -#define M4_MOTOR_MAP AXIS_X // 1ma -#define M4_STEP_ANGLE 1.8 // 1sa -#define M4_TRAVEL_PER_REV 5.08 // 1tr -#define M4_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M4_POLARITY 0 // 1po 0=normal, 1=reversed -#define M4_POWER_MODE MOTOR_ALWAYS_POWERED// 1pm TRUE=low power idle enabled -#define M4_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Y -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 5.08 // 1tr -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 -#define M3_POWER_MODE MOTOR_ALWAYS_POWERED -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M2_MOTOR_MAP AXIS_Z +#define M4_MOTOR_MAP AXIS_X // 1ma +#define M4_STEP_ANGLE 1.8 // 1sa +#define M4_TRAVEL_PER_REV 5.08 // 1tr +#define M4_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M4_POLARITY 0 // 1po 0=normal, 1=reversed +#define M4_POWER_MODE MOTOR_ALWAYS_POWERED// 1pm TRUE=low power idle enabled +#define M4_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Y +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 5.08 // 1tr +#define M3_MICROSTEPS 8 +#define M3_POLARITY 1 +#define M3_POWER_MODE MOTOR_ALWAYS_POWERED +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M2_MOTOR_MAP AXIS_Z #if HAS_CANSTACK_Z_AXIS -#define M2_STEP_ANGLE 15 -#define M2_TRAVEL_PER_REV 1.27254 +#define M2_STEP_ANGLE 15 +#define M2_TRAVEL_PER_REV 1.27254 #else -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 5.08 +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 5.08 #endif -#define M2_MICROSTEPS 8 -#define M2_POLARITY 1 -#define M2_POWER_MODE MOTOR_ALWAYS_POWERED -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M1_MOTOR_MAP AXIS_A -#define M1_STEP_ANGLE 1.8 -#define M1_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M1_MICROSTEPS 8 -#define M1_POLARITY 1 -#define M1_POWER_MODE MOTOR_ALWAYS_POWERED -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M5_POWER_LEVEL MOTOR_POWER_LEVEL -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M2_MICROSTEPS 8 +#define M2_POLARITY 1 +#define M2_POWER_MODE MOTOR_ALWAYS_POWERED +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M1_MOTOR_MAP AXIS_A +#define M1_STEP_ANGLE 1.8 +#define M1_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M1_MICROSTEPS 8 +#define M1_POLARITY 1 +#define M1_POWER_MODE MOTOR_ALWAYS_POWERED +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M5_POWER_LEVEL MOTOR_POWER_LEVEL +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -//#define X_AXIS_MODE AXIS_DISABLED // DIAGNOSTIC TEST ONLY!!! -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 138 // xtr travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX/3) // xsv -#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 0 // xzb mm -#define X_JERK_HOMING JERK_HOMING // xjh - -//#define Y_AXIS_MODE AXIS_DISABLED // DIAGNOSTIC TEST ONLY!!! -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX X_VELOCITY_MAX -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 115 -#define Y_JERK_MAX JERK_MAX -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX/3) -#define Y_LATCH_VELOCITY LATCH_VELOCITY -#define Y_LATCH_BACKOFF 5 -#define Y_ZERO_BACKOFF 0 -#define Y_JERK_HOMING JERK_HOMING - -#define Z_AXIS_MODE AXIS_STANDARD +//#define X_AXIS_MODE AXIS_DISABLED // DIAGNOSTIC TEST ONLY!!! +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 1500 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits +#define X_TRAVEL_MAX 138 // xtr travel between switches or crashes +#define X_JERK_MAX JERK_MAX // xjm +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY (X_FEEDRATE_MAX/3) // xsv +#define X_LATCH_VELOCITY LATCH_VELOCITY // xlv mm/min +#define X_LATCH_BACKOFF 5 // xlb mm +#define X_ZERO_BACKOFF 0 // xzb mm +#define X_JERK_HOMING JERK_HOMING // xjh + +//#define Y_AXIS_MODE AXIS_DISABLED // DIAGNOSTIC TEST ONLY!!! +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX X_VELOCITY_MAX +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 115 +#define Y_JERK_MAX JERK_MAX +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Y_SEARCH_VELOCITY (Y_FEEDRATE_MAX/3) +#define Y_LATCH_VELOCITY LATCH_VELOCITY +#define Y_LATCH_BACKOFF 5 +#define Y_ZERO_BACKOFF 0 +#define Y_JERK_HOMING JERK_HOMING + +#define Z_AXIS_MODE AXIS_STANDARD #if HAS_CANSTACK_Z_AXIS -#define Z_VELOCITY_MAX 1000 +#define Z_VELOCITY_MAX 1000 #else -#define Z_VELOCITY_MAX X_VELOCITY_MAX +#define Z_VELOCITY_MAX X_VELOCITY_MAX #endif -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN -75 -#define Z_TRAVEL_MAX 0 -#define Z_JERK_MAX JERK_MAX // 200 million -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX/3) -#define Z_LATCH_VELOCITY LATCH_VELOCITY -#define Z_LATCH_BACKOFF 5 -#define Z_ZERO_BACKOFF 0 -#define Z_JERK_HOMING JERK_HOMING +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN -75 +#define Z_TRAVEL_MAX 0 +#define Z_JERK_MAX JERK_MAX // 200 million +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY (Z_FEEDRATE_MAX/3) +#define Z_LATCH_VELOCITY LATCH_VELOCITY +#define Z_LATCH_BACKOFF 5 +#define Z_ZERO_BACKOFF 0 +#define Z_JERK_HOMING JERK_HOMING // A values are chosen to make the A motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MAX 0 // max=0 min=0 means infinite, no limit -#define A_TRAVEL_MIN 0 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MAX 0 -#define B_TRAVEL_MIN 0 -#define B_JERK_MAX JERK_MAX -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MAX 0 -#define C_TRAVEL_MIN 0 -#define C_JERK_MAX JERK_MAX -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 +#define A_AXIS_MODE AXIS_RADIUS +#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MAX 0 // max=0 min=0 means infinite, no limit +#define A_TRAVEL_MIN 0 +#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX + +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX 3600 +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MAX 0 +#define B_TRAVEL_MIN 0 +#define B_JERK_MAX JERK_MAX +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS 1 + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX 3600 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MAX 0 +#define C_TRAVEL_MIN 0 +#define C_JERK_MAX JERK_MAX +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS 1 // *** PWM SPINDLE CONTROL *** -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 2000 -#define P1_CW_PHASE_LO 0.125 // phase [0..1] -#define P1_CW_PHASE_HI 0.2 -#define P1_CCW_SPEED_LO 1000 -#define P1_CCW_SPEED_HI 2000 -#define P1_CCW_PHASE_LO 0.125 -#define P1_CCW_PHASE_HI 0.2 -#define P1_PWM_PHASE_OFF 0.1 +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 2000 +#define P1_CW_PHASE_LO 0.125 // phase [0..1] +#define P1_CW_PHASE_HI 0.2 +#define P1_CCW_SPEED_LO 1000 +#define P1_CCW_SPEED_HI 2000 +#define P1_CCW_PHASE_LO 0.125 +#define P1_CCW_PHASE_HI 0.2 +#define P1_PWM_PHASE_OFF 0.1 // *** DEFAULT COORDINATE SYSTEM OFFSETS *** -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET 0 // but the again, so is everyting else (at least for start) +#define G55_X_OFFSET 0 // but the again, so is everyting else (at least for start) #define G55_Y_OFFSET 0 #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 diff --git a/src/settings/settings_zen7x12.h b/src/settings/settings_zen7x12.h index 3c97c54..95bbd79 100755 --- a/src/settings/settings_zen7x12.h +++ b/src/settings/settings_zen7x12.h @@ -25,8 +25,8 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Note: The values in this file are the default settings that are loaded - * into a virgin EEPROM, and can be changed using the config commands. - * After initial load the EEPROM values (or changed values) are used. + * into a virgin EEPROM, and can be changed using the config commands. + * After initial load the EEPROM values (or changed values) are used. */ /***********************************************************************/ @@ -36,180 +36,180 @@ // ***> NOTE: The init message must be a single line with no CRs or LFs #define INIT_MESSAGE "Initializing configs to Zen Toolworks 7x12 profile" -#define JERK_MAX_LINEAR 500 // 500,000,000 mm/(min^3) -#define JERK_MAX_ROTARY 10000 // 10 billion mm/(min^3) -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners +#define JERK_MAX_LINEAR 500 // 500,000,000 mm/(min^3) +#define JERK_MAX_ROTARY 10000 // 10 billion mm/(min^3) +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners // *** settings.h overrides *** #undef COMM_MODE -#define COMM_MODE JSON_MODE +#define COMM_MODE JSON_MODE #undef JSON_VERBOSITY -#define JSON_VERBOSITY JV_VERBOSE +#define JSON_VERBOSITY JV_VERBOSE #undef SWITCH_TYPE -#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN +#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN // *** motor settings *** -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 1.25 // 1tr -#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 -#define M1_POLARITY 1 // REVERSE// 1po 0=normal, 1=reverse -#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp - -#define M2_MOTOR_MAP AXIS_Y -#define M2_STEP_ANGLE 1.8 -#define M2_TRAVEL_PER_REV 1.25 -#define M2_MICROSTEPS 8 -#define M2_POLARITY 0 -#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M2_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M3_MOTOR_MAP AXIS_Z -#define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 1.25 -#define M3_MICROSTEPS 8 -#define M3_POLARITY 1 // REVERSE -#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M3_POWER_LEVEL MOTOR_POWER_LEVEL - -#define M4_MOTOR_MAP AXIS_A -#define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev -#define M4_MICROSTEPS 8 -#define M4_POLARITY 0 -#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE -#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 8 -#define M5_POLARITY 0 -#define M5_POWER_MODE MOTOR_POWERED_IN_CYCLE -#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 8 -#define M6_POLARITY 0 -#define M6_POWER_MODE MOTOR_POWERED_IN_CYCLE -#define M6_POWER_LEVEL MOTOR_POWER_LEVEL +#define M1_MOTOR_MAP AXIS_X // 1ma +#define M1_STEP_ANGLE 1.8 // 1sa +#define M1_TRAVEL_PER_REV 1.25 // 1tr +#define M1_MICROSTEPS 8 // 1mi 1,2,4,8 +#define M1_POLARITY 1 // REVERSE// 1po 0=normal, 1=reverse +#define M1_POWER_MODE MOTOR_POWERED_IN_CYCLE // 1pm standard +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp + +#define M2_MOTOR_MAP AXIS_Y +#define M2_STEP_ANGLE 1.8 +#define M2_TRAVEL_PER_REV 1.25 +#define M2_MICROSTEPS 8 +#define M2_POLARITY 0 +#define M2_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define M2_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M3_MOTOR_MAP AXIS_Z +#define M3_STEP_ANGLE 1.8 +#define M3_TRAVEL_PER_REV 1.25 +#define M3_MICROSTEPS 8 +#define M3_POLARITY 1 // REVERSE +#define M3_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define M3_POWER_LEVEL MOTOR_POWER_LEVEL + +#define M4_MOTOR_MAP AXIS_A +#define M4_STEP_ANGLE 1.8 +#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M4_MICROSTEPS 8 +#define M4_POLARITY 0 +#define M4_POWER_MODE MOTOR_POWERED_IN_CYCLE +#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 8 +#define M5_POLARITY 0 +#define M5_POWER_MODE MOTOR_POWERED_IN_CYCLE +#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 8 +#define M6_POLARITY 0 +#define M6_POWER_MODE MOTOR_POWERED_IN_CYCLE +#define M6_POWER_LEVEL MOTOR_POWER_LEVEL // *** axis settings *** -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX 600 // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing -#define X_TRAVEL_MAX 475 // xtm maximum travel - used by soft limits and homing -#define X_JERK_MAX JERK_MAX_LINEAR // xjm -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_LIMIT // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -//#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SEARCH_VELOCITY 500 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 2 // xlb mm -#define X_ZERO_BACKOFF 1 // xzb mm -#define X_JERK_HOMING X_JERK_MAX // xjh - -#define Y_AXIS_MODE AXIS_STANDARD -#define Y_VELOCITY_MAX 600 -#define Y_FEEDRATE_MAX Y_VELOCITY_MAX -#define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 200 -#define Y_JERK_MAX JERK_MAX_LINEAR -#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SWITCH_MODE_MIN SW_MODE_HOMING -#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT -//#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED -#define Y_SEARCH_VELOCITY 500 -#define Y_LATCH_VELOCITY 100 -#define Y_LATCH_BACKOFF 2 -#define Y_ZERO_BACKOFF 1 -#define Y_JERK_HOMING Y_JERK_MAX - -#define Z_AXIS_MODE AXIS_STANDARD -#define Z_VELOCITY_MAX 500 -#define Z_FEEDRATE_MAX Z_VELOCITY_MAX -#define Z_TRAVEL_MIN 0 -#define Z_TRAVEL_MAX 75 -#define Z_JERK_MAX JERK_MAX_LINEAR -#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED -#define Z_SWITCH_MODE_MAX SW_MODE_HOMING -#define Z_SEARCH_VELOCITY 400 -#define Z_LATCH_VELOCITY 100 -#define Z_LATCH_BACKOFF 2 -#define Z_ZERO_BACKOFF 1 -#define Z_JERK_HOMING Z_JERK_MAX +#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values +#define X_VELOCITY_MAX 600 // xvm G0 max velocity in mm/min +#define X_FEEDRATE_MAX X_VELOCITY_MAX // xfr G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // xtn minimum travel - used by soft limits and homing +#define X_TRAVEL_MAX 475 // xtm maximum travel - used by soft limits and homing +#define X_JERK_MAX JERK_MAX_LINEAR // xjm +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd +#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_LIMIT // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +//#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SEARCH_VELOCITY 500 // xsv move in negative direction +#define X_LATCH_VELOCITY 100 // xlv mm/min +#define X_LATCH_BACKOFF 2 // xlb mm +#define X_ZERO_BACKOFF 1 // xzb mm +#define X_JERK_HOMING X_JERK_MAX // xjh + +#define Y_AXIS_MODE AXIS_STANDARD +#define Y_VELOCITY_MAX 600 +#define Y_FEEDRATE_MAX Y_VELOCITY_MAX +#define Y_TRAVEL_MIN 0 +#define Y_TRAVEL_MAX 200 +#define Y_JERK_MAX JERK_MAX_LINEAR +#define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Y_SWITCH_MODE_MIN SW_MODE_HOMING +#define Y_SWITCH_MODE_MAX SW_MODE_LIMIT +//#define Y_SWITCH_MODE_MAX SW_MODE_DISABLED +#define Y_SEARCH_VELOCITY 500 +#define Y_LATCH_VELOCITY 100 +#define Y_LATCH_BACKOFF 2 +#define Y_ZERO_BACKOFF 1 +#define Y_JERK_HOMING Y_JERK_MAX + +#define Z_AXIS_MODE AXIS_STANDARD +#define Z_VELOCITY_MAX 500 +#define Z_FEEDRATE_MAX Z_VELOCITY_MAX +#define Z_TRAVEL_MIN 0 +#define Z_TRAVEL_MAX 75 +#define Z_JERK_MAX JERK_MAX_LINEAR +#define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define Z_SWITCH_MODE_MIN SW_MODE_DISABLED +#define Z_SWITCH_MODE_MAX SW_MODE_HOMING +#define Z_SEARCH_VELOCITY 400 +#define Z_LATCH_VELOCITY 100 +#define Z_LATCH_BACKOFF 2 +#define Z_ZERO_BACKOFF 1 +#define Z_JERK_HOMING Z_JERK_MAX // Rotary values are chosen to make the motor react the same as X for testing -#define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis -#define A_FEEDRATE_MAX A_VELOCITY_MAX -#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit -#define A_TRAVEL_MAX -1 -#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define A_SWITCH_MODE_MIN SW_MODE_HOMING -#define A_SWITCH_MODE_MAX SW_MODE_DISABLED -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 -#define A_JERK_HOMING A_JERK_MAX - -#define B_AXIS_MODE AXIS_RADIUS -#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define B_SWITCH_MODE_MIN SW_MODE_HOMING -#define B_SWITCH_MODE_MAX SW_MODE_DISABLED -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 -#define B_JERK_HOMING B_JERK_MAX - -#define C_AXIS_MODE AXIS_RADIUS -#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) -#define C_SWITCH_MODE_MIN SW_MODE_HOMING -#define C_SWITCH_MODE_MAX SW_MODE_DISABLED -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 -#define C_JERK_HOMING C_JERK_MAX +#define A_AXIS_MODE AXIS_RADIUS +#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +#define A_FEEDRATE_MAX A_VELOCITY_MAX +#define A_TRAVEL_MIN -1 // min/max the same means infinite, no limit +#define A_TRAVEL_MAX -1 +#define A_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define A_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define A_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define A_SWITCH_MODE_MIN SW_MODE_HOMING +#define A_SWITCH_MODE_MAX SW_MODE_DISABLED +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 +#define A_JERK_HOMING A_JERK_MAX + +#define B_AXIS_MODE AXIS_RADIUS +#define B_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define B_SWITCH_MODE_MIN SW_MODE_HOMING +#define B_SWITCH_MODE_MAX SW_MODE_DISABLED +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 +#define B_JERK_HOMING B_JERK_MAX + +#define C_AXIS_MODE AXIS_RADIUS +#define C_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX (X_JERK_MAX*(360/M1_TRAVEL_PER_REV)) +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS (M1_TRAVEL_PER_REV/(2*3.14159628)) +#define C_SWITCH_MODE_MIN SW_MODE_HOMING +#define C_SWITCH_MODE_MAX SW_MODE_DISABLED +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 +#define C_JERK_HOMING C_JERK_MAX // *** DEFAULT COORDINATE SYSTEM OFFSETS *** -#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros +#define G54_X_OFFSET 0 // G54 is traditionally set to all zeros #define G54_Y_OFFSET 0 #define G54_Z_OFFSET 0 #define G54_A_OFFSET 0 #define G54_B_OFFSET 0 #define G54_C_OFFSET 0 -#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table +#define G55_X_OFFSET (X_TRAVEL_MAX/2) // set to middle of table #define G55_Y_OFFSET (Y_TRAVEL_MAX/2) #define G55_Z_OFFSET 0 #define G55_A_OFFSET 0 diff --git a/src/spindle.c b/src/spindle.c index cc24d88..0237e7c 100755 --- a/src/spindle.c +++ b/src/spindle.c @@ -25,8 +25,8 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "tinyg.h" // #1 -#include "config.h" // #2 +#include "tinyg.h" // #1 +#include "config.h" // #2 #include "spindle.h" #include "gpio.h" #include "planner.h" @@ -41,8 +41,8 @@ static void _exec_spindle_speed(float *value, float *flag); */ void cm_spindle_init() { - if( pwm.c[PWM_1].frequency < 0 ) - pwm.c[PWM_1].frequency = 0; + if( pwm.c[PWM_1].frequency < 0 ) + pwm.c[PWM_1].frequency = 0; pwm_set_freq(PWM_1, pwm.c[PWM_1].frequency); pwm_set_duty(PWM_1, pwm.c[PWM_1].phase_off); @@ -53,30 +53,30 @@ void cm_spindle_init() */ float cm_get_spindle_pwm( uint8_t spindle_mode ) { - float speed_lo=0, speed_hi=0, phase_lo=0, phase_hi=0; - if (spindle_mode == SPINDLE_CW ) { - speed_lo = pwm.c[PWM_1].cw_speed_lo; - speed_hi = pwm.c[PWM_1].cw_speed_hi; - phase_lo = pwm.c[PWM_1].cw_phase_lo; - phase_hi = pwm.c[PWM_1].cw_phase_hi; - } else if (spindle_mode == SPINDLE_CCW ) { - speed_lo = pwm.c[PWM_1].ccw_speed_lo; - speed_hi = pwm.c[PWM_1].ccw_speed_hi; - phase_lo = pwm.c[PWM_1].ccw_phase_lo; - phase_hi = pwm.c[PWM_1].ccw_phase_hi; - } - - if (spindle_mode==SPINDLE_CW || spindle_mode==SPINDLE_CCW ) { - // clamp spindle speed to lo/hi range - if( cm.gm.spindle_speed < speed_lo ) cm.gm.spindle_speed = speed_lo; - if( cm.gm.spindle_speed > speed_hi ) cm.gm.spindle_speed = speed_hi; - - // normalize speed to [0..1] - float speed = (cm.gm.spindle_speed - speed_lo) / (speed_hi - speed_lo); - return (speed * (phase_hi - phase_lo)) + phase_lo; - } else { - return pwm.c[PWM_1].phase_off; - } + float speed_lo=0, speed_hi=0, phase_lo=0, phase_hi=0; + if (spindle_mode == SPINDLE_CW ) { + speed_lo = pwm.c[PWM_1].cw_speed_lo; + speed_hi = pwm.c[PWM_1].cw_speed_hi; + phase_lo = pwm.c[PWM_1].cw_phase_lo; + phase_hi = pwm.c[PWM_1].cw_phase_hi; + } else if (spindle_mode == SPINDLE_CCW ) { + speed_lo = pwm.c[PWM_1].ccw_speed_lo; + speed_hi = pwm.c[PWM_1].ccw_speed_hi; + phase_lo = pwm.c[PWM_1].ccw_phase_lo; + phase_hi = pwm.c[PWM_1].ccw_phase_hi; + } + + if (spindle_mode==SPINDLE_CW || spindle_mode==SPINDLE_CCW ) { + // clamp spindle speed to lo/hi range + if( cm.gm.spindle_speed < speed_lo ) cm.gm.spindle_speed = speed_lo; + if( cm.gm.spindle_speed > speed_hi ) cm.gm.spindle_speed = speed_hi; + + // normalize speed to [0..1] + float speed = (cm.gm.spindle_speed - speed_lo) / (speed_hi - speed_lo); + return speed * (phase_hi - phase_lo) + phase_lo; + } else { + return pwm.c[PWM_1].phase_off; + } } /* @@ -86,53 +86,53 @@ float cm_get_spindle_pwm( uint8_t spindle_mode ) stat_t cm_spindle_control(uint8_t spindle_mode) { - float value[AXES] = { (float)spindle_mode, 0,0,0,0,0 }; - mp_queue_command(_exec_spindle_control, value, value); - return(STAT_OK); + float value[AXES] = { (float)spindle_mode, 0,0,0,0,0 }; + mp_queue_command(_exec_spindle_control, value, value); + return(STAT_OK); } //static void _exec_spindle_control(uint8_t spindle_mode, float f, float *vector, float *flag) static void _exec_spindle_control(float *value, float *flag) { - uint8_t spindle_mode = (uint8_t)value[0]; - cm_set_spindle_mode(MODEL, spindle_mode); + uint8_t spindle_mode = (uint8_t)value[0]; + cm_set_spindle_mode(MODEL, spindle_mode); if (spindle_mode == SPINDLE_CW) { - gpio_set_bit_on(SPINDLE_BIT); - gpio_set_bit_off(SPINDLE_DIR); - } else if (spindle_mode == SPINDLE_CCW) { - gpio_set_bit_on(SPINDLE_BIT); - gpio_set_bit_on(SPINDLE_DIR); - } else { - gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop - } - - // PWM spindle control - pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode) ); + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_off(SPINDLE_DIR); + } else if (spindle_mode == SPINDLE_CCW) { + gpio_set_bit_on(SPINDLE_BIT); + gpio_set_bit_on(SPINDLE_DIR); + } else { + gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop + } + + // PWM spindle control + pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode) ); } /* - * cm_set_spindle_speed() - queue the S parameter to the planner buffer - * cm_exec_spindle_speed() - execute the S command (called from the planner buffer) - * _exec_spindle_speed() - spindle speed callback from planner queue + * cm_set_spindle_speed() - queue the S parameter to the planner buffer + * cm_exec_spindle_speed() - execute the S command (called from the planner buffer) + * _exec_spindle_speed() - spindle speed callback from planner queue */ stat_t cm_set_spindle_speed(float speed) { -// if (speed > cfg.max_spindle speed) -// return (STAT_MAX_SPINDLE_SPEED_EXCEEDED); +// if (speed > cfg.max_spindle speed) +// return STAT_MAX_SPINDLE_SPEED_EXCEEDED; - float value[AXES] = { speed, 0,0,0,0,0 }; - mp_queue_command(_exec_spindle_speed, value, value); - return (STAT_OK); + float value[AXES] = { speed, 0,0,0,0,0 }; + mp_queue_command(_exec_spindle_speed, value, value); + return STAT_OK; } void cm_exec_spindle_speed(float speed) { - cm_set_spindle_speed(speed); + cm_set_spindle_speed(speed); } static void _exec_spindle_speed(float *value, float *flag) { - cm_set_spindle_speed_parameter(MODEL, value[0]); - pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode) ); // update spindle speed if we're running + cm_set_spindle_speed_parameter(MODEL, value[0]); + pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode) ); // update spindle speed if we're running } diff --git a/src/spindle.h b/src/spindle.h index 731cd91..431a73d 100755 --- a/src/spindle.h +++ b/src/spindle.h @@ -30,10 +30,10 @@ void cm_spindle_init(); -stat_t cm_set_spindle_speed(float speed); // S parameter -void cm_exec_spindle_speed(float speed); // callback for above +stat_t cm_set_spindle_speed(float speed); // S parameter +void cm_exec_spindle_speed(float speed); // callback for above -stat_t cm_spindle_control(uint8_t spindle_mode); // M3, M4, M5 integrated spindle control -void cm_exec_spindle_control(uint8_t spindle_mode); // callback for above +stat_t cm_spindle_control(uint8_t spindle_mode); // M3, M4, M5 integrated spindle control +void cm_exec_spindle_control(uint8_t spindle_mode); // callback for above -#endif // End of include guard: SPINDLE_H_ONCE +#endif // End of include guard: SPINDLE_H_ONCE diff --git a/src/status.c b/src/status.c new file mode 100644 index 0000000..30dedb6 --- /dev/null +++ b/src/status.c @@ -0,0 +1,346 @@ +/* + * status.c - TinyG - An embedded rs274/ngc CNC controller + * This file is part of the TinyG project. + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2013 - 2015 Robert Giseburt + * + * This file ("the software") is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License, version 2 as published by the + * Free Software Foundation. You should have received a copy of the GNU General Public + * License, version 2 along with the software. If not, see . + * + * 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 "tinyg.h" + +#include + +/**** Status Messages *************************************************************** + * get_status_message() - return the status message + * + * See tinyg.h for status codes. These strings must align with the status codes in tinyg.h + * The number of elements in the indexing array must match the # of strings + * + * Reference for putting display strings and string arrays in AVR program memory: + * http://www.cs.mun.ca/~paul/cs4723/material/atmel/avr-libc-user-manual-1.6.5/pgmspace.html + */ + +stat_t status_code; // allocate a variable for the ritorno macro +char global_string_buf[MESSAGE_LEN]; // allocate a string for global message use + +/*** Status message strings ***/ + +static const char stat_00[] PROGMEM = "OK"; +static const char stat_01[] PROGMEM = "Error"; +static const char stat_02[] PROGMEM = "Eagain"; +static const char stat_03[] PROGMEM = "Noop"; +static const char stat_04[] PROGMEM = "Complete"; +static const char stat_05[] PROGMEM = "Terminated"; +static const char stat_06[] PROGMEM = "Hard reset"; +static const char stat_07[] PROGMEM = "End of line"; +static const char stat_08[] PROGMEM = "End of file"; +static const char stat_09[] PROGMEM = "File not open"; + +static const char stat_10[] PROGMEM = "Max file size exceeded"; +static const char stat_11[] PROGMEM = "No such device"; +static const char stat_12[] PROGMEM = "Buffer empty"; +static const char stat_13[] PROGMEM = "Buffer full"; +static const char stat_14[] PROGMEM = "Buffer full - fatal"; +static const char stat_15[] PROGMEM = "Initializing"; +static const char stat_16[] PROGMEM = "Entering boot loader"; +static const char stat_17[] PROGMEM = "Function is stubbed"; +static const char stat_18[] PROGMEM = "18"; +static const char stat_19[] PROGMEM = "19"; + +static const char stat_20[] PROGMEM = "Internal error"; +static const char stat_21[] PROGMEM = "Internal range error"; +static const char stat_22[] PROGMEM = "Floating point error"; +static const char stat_23[] PROGMEM = "Divide by zero"; +static const char stat_24[] PROGMEM = "Invalid Address"; +static const char stat_25[] PROGMEM = "Read-only address"; +static const char stat_26[] PROGMEM = "Initialization failure"; +static const char stat_27[] PROGMEM = "System alarm - shutting down"; +static const char stat_28[] PROGMEM = "Failed to get planner buffer"; +static const char stat_29[] PROGMEM = "Generic exception report"; + +static const char stat_30[] PROGMEM = "Move time is infinite"; +static const char stat_31[] PROGMEM = "Move time is NAN"; +static const char stat_32[] PROGMEM = "Float is infinite"; +static const char stat_33[] PROGMEM = "Float is NAN"; +static const char stat_34[] PROGMEM = "Persistence error"; +static const char stat_35[] PROGMEM = "Bad status report setting"; +static const char stat_36[] PROGMEM = "36"; +static const char stat_37[] PROGMEM = "37"; +static const char stat_38[] PROGMEM = "38"; +static const char stat_39[] PROGMEM = "39"; + +static const char stat_40[] PROGMEM = "40"; +static const char stat_41[] PROGMEM = "41"; +static const char stat_42[] PROGMEM = "42"; +static const char stat_43[] PROGMEM = "43"; +static const char stat_44[] PROGMEM = "44"; +static const char stat_45[] PROGMEM = "45"; +static const char stat_46[] PROGMEM = "46"; +static const char stat_47[] PROGMEM = "47"; +static const char stat_48[] PROGMEM = "48"; +static const char stat_49[] PROGMEM = "49"; +static const char stat_50[] PROGMEM = "50"; +static const char stat_51[] PROGMEM = "51"; +static const char stat_52[] PROGMEM = "52"; +static const char stat_53[] PROGMEM = "53"; +static const char stat_54[] PROGMEM = "54"; +static const char stat_55[] PROGMEM = "55"; +static const char stat_56[] PROGMEM = "56"; +static const char stat_57[] PROGMEM = "57"; +static const char stat_58[] PROGMEM = "58"; +static const char stat_59[] PROGMEM = "59"; +static const char stat_60[] PROGMEM = "60"; +static const char stat_61[] PROGMEM = "61"; +static const char stat_62[] PROGMEM = "62"; +static const char stat_63[] PROGMEM = "63"; +static const char stat_64[] PROGMEM = "64"; +static const char stat_65[] PROGMEM = "65"; +static const char stat_66[] PROGMEM = "66"; +static const char stat_67[] PROGMEM = "67"; +static const char stat_68[] PROGMEM = "68"; +static const char stat_69[] PROGMEM = "69"; +static const char stat_70[] PROGMEM = "70"; +static const char stat_71[] PROGMEM = "71"; +static const char stat_72[] PROGMEM = "72"; +static const char stat_73[] PROGMEM = "73"; +static const char stat_74[] PROGMEM = "74"; +static const char stat_75[] PROGMEM = "75"; +static const char stat_76[] PROGMEM = "76"; +static const char stat_77[] PROGMEM = "77"; +static const char stat_78[] PROGMEM = "78"; +static const char stat_79[] PROGMEM = "79"; +static const char stat_80[] PROGMEM = "80"; +static const char stat_81[] PROGMEM = "81"; +static const char stat_82[] PROGMEM = "82"; +static const char stat_83[] PROGMEM = "83"; +static const char stat_84[] PROGMEM = "84"; +static const char stat_85[] PROGMEM = "85"; +static const char stat_86[] PROGMEM = "86"; +static const char stat_87[] PROGMEM = "87"; +static const char stat_88[] PROGMEM = "88"; +static const char stat_89[] PROGMEM = "89"; + +static const char stat_90[] PROGMEM = "Config sub-system assertion failure"; +static const char stat_91[] PROGMEM = "IO sub-system assertion failure"; +static const char stat_92[] PROGMEM = "Encoder assertion failure"; +static const char stat_93[] PROGMEM = "Stepper assertion failure"; +static const char stat_94[] PROGMEM = "Planner assertion failure"; +static const char stat_95[] PROGMEM = "Canonical machine assertion failure"; +static const char stat_96[] PROGMEM = "Controller assertion failure"; +static const char stat_97[] PROGMEM = "Stack overflow detected"; +static const char stat_98[] PROGMEM = "Memory fault detected"; +static const char stat_99[] PROGMEM = "Generic assertion failure"; + +static const char stat_100[] PROGMEM = "Unrecognized command or config name"; +static const char stat_101[] PROGMEM = "Invalid or malformed command"; +static const char stat_102[] PROGMEM = "Bad number format"; +static const char stat_103[] PROGMEM = "Unsupported number or JSON type"; +static const char stat_104[] PROGMEM = "Parameter is read-only"; +static const char stat_105[] PROGMEM = "Parameter cannot be read"; +static const char stat_106[] PROGMEM = "Command not accepted at this time"; +static const char stat_107[] PROGMEM = "Input exceeds max length"; +static const char stat_108[] PROGMEM = "Input less than minimum value"; +static const char stat_109[] PROGMEM = "Input exceeds maximum value"; + +static const char stat_110[] PROGMEM = "Input value range error"; +static const char stat_111[] PROGMEM = "JSON syntax error"; +static const char stat_112[] PROGMEM = "JSON input has too many pairs"; +static const char stat_113[] PROGMEM = "JSON string too long"; +static const char stat_114[] PROGMEM = "114"; +static const char stat_115[] PROGMEM = "115"; +static const char stat_116[] PROGMEM = "116"; +static const char stat_117[] PROGMEM = "117"; +static const char stat_118[] PROGMEM = "118"; +static const char stat_119[] PROGMEM = "119"; + +static const char stat_120[] PROGMEM = "120"; +static const char stat_121[] PROGMEM = "121"; +static const char stat_122[] PROGMEM = "122"; +static const char stat_123[] PROGMEM = "123"; +static const char stat_124[] PROGMEM = "124"; +static const char stat_125[] PROGMEM = "125"; +static const char stat_126[] PROGMEM = "126"; +static const char stat_127[] PROGMEM = "127"; +static const char stat_128[] PROGMEM = "128"; +static const char stat_129[] PROGMEM = "129"; + +static const char stat_130[] PROGMEM = "Generic Gcode input error"; +static const char stat_131[] PROGMEM = "Gcode command unsupported"; +static const char stat_132[] PROGMEM = "M code unsupported"; +static const char stat_133[] PROGMEM = "Gcode modal group violation"; +static const char stat_134[] PROGMEM = "Axis word missing"; +static const char stat_135[] PROGMEM = "Axis cannot be present"; +static const char stat_136[] PROGMEM = "Axis is invalid for this command"; +static const char stat_137[] PROGMEM = "Axis is disabled"; +static const char stat_138[] PROGMEM = "Axis target position is missing"; +static const char stat_139[] PROGMEM = "Axis target position is invalid"; + +static const char stat_140[] PROGMEM = "Selected plane is missing"; +static const char stat_141[] PROGMEM = "Selected plane is invalid"; +static const char stat_142[] PROGMEM = "Feedrate not specified"; +static const char stat_143[] PROGMEM = "Inverse time mode cannot be used with this command"; +static const char stat_144[] PROGMEM = "Rotary axes cannot be used with this command"; +static const char stat_145[] PROGMEM = "G0 or G1 must be active for G53"; +static const char stat_146[] PROGMEM = "Requested velocity exceeds limits"; +static const char stat_147[] PROGMEM = "Cutter compensation cannot be enabled"; +static const char stat_148[] PROGMEM = "Programmed point same as current point"; +static const char stat_149[] PROGMEM = "Spindle speed below minimum"; + +static const char stat_150[] PROGMEM = "Spindle speed exceeded maximum"; +static const char stat_151[] PROGMEM = "Spindle S word is missing"; +static const char stat_152[] PROGMEM = "Spindle S word is invalid"; +static const char stat_153[] PROGMEM = "Spindle must be off for this command"; +static const char stat_154[] PROGMEM = "Spindle must be turning for this command"; +static const char stat_155[] PROGMEM = "Arc specification error"; +static const char stat_156[] PROGMEM = "Arc specification error - missing axis(es)"; +static const char stat_157[] PROGMEM = "Arc specification error - missing offset(s)"; +static const char stat_158[] PROGMEM = "Arc specification error - radius arc out of tolerance"; // current longest message: 56 chard +static const char stat_159[] PROGMEM = "Arc specification error - endpoint is starting point"; + +static const char stat_160[] PROGMEM = "P word is missing"; +static const char stat_161[] PROGMEM = "P word is invalid"; +static const char stat_162[] PROGMEM = "P word is zero"; +static const char stat_163[] PROGMEM = "P word is negative"; +static const char stat_164[] PROGMEM = "P word is not an integer"; +static const char stat_165[] PROGMEM = "P word is not a valid tool number"; +static const char stat_166[] PROGMEM = "D word is missing"; +static const char stat_167[] PROGMEM = "D word is invalid"; +static const char stat_168[] PROGMEM = "E word is missing"; +static const char stat_169[] PROGMEM = "E word is invalid"; + +static const char stat_170[] PROGMEM = "H word is missing"; +static const char stat_171[] PROGMEM = "H word is invalid"; +static const char stat_172[] PROGMEM = "L word is missing"; +static const char stat_173[] PROGMEM = "L word is invalid"; +static const char stat_174[] PROGMEM = "Q word is missing"; +static const char stat_175[] PROGMEM = "Q word is invalid"; +static const char stat_176[] PROGMEM = "R word is missing"; +static const char stat_177[] PROGMEM = "R word is invalid"; +static const char stat_178[] PROGMEM = "T word is missing"; +static const char stat_179[] PROGMEM = "T word is invalid"; + +static const char stat_180[] PROGMEM = "180"; +static const char stat_181[] PROGMEM = "181"; +static const char stat_182[] PROGMEM = "182"; +static const char stat_183[] PROGMEM = "183"; +static const char stat_184[] PROGMEM = "184"; +static const char stat_185[] PROGMEM = "185"; +static const char stat_186[] PROGMEM = "186"; +static const char stat_187[] PROGMEM = "187"; +static const char stat_188[] PROGMEM = "188"; +static const char stat_189[] PROGMEM = "189"; + +static const char stat_190[] PROGMEM = "190"; +static const char stat_191[] PROGMEM = "191"; +static const char stat_192[] PROGMEM = "192"; +static const char stat_193[] PROGMEM = "193"; +static const char stat_194[] PROGMEM = "194"; +static const char stat_195[] PROGMEM = "195"; +static const char stat_196[] PROGMEM = "196"; +static const char stat_197[] PROGMEM = "197"; +static const char stat_198[] PROGMEM = "198"; +static const char stat_199[] PROGMEM = "199"; + +static const char stat_200[] PROGMEM = "Generic TinyG error"; +static const char stat_201[] PROGMEM = "Move less than minimum length"; +static const char stat_202[] PROGMEM = "Move less than minimum time"; +static const char stat_203[] PROGMEM = "Machine is alarmed - Command not processed"; // current longest message 43 chars (including 0) +static const char stat_204[] PROGMEM = "Limit switch hit - Shutdown occurred"; +static const char stat_205[] PROGMEM = "Trapezoid planner failed to converge"; +static const char stat_206[] PROGMEM = "206"; +static const char stat_207[] PROGMEM = "207"; +static const char stat_208[] PROGMEM = "208"; +static const char stat_209[] PROGMEM = "209"; + +static const char stat_210[] PROGMEM = "210"; +static const char stat_211[] PROGMEM = "211"; +static const char stat_212[] PROGMEM = "212"; +static const char stat_213[] PROGMEM = "213"; +static const char stat_214[] PROGMEM = "214"; +static const char stat_215[] PROGMEM = "215"; +static const char stat_216[] PROGMEM = "216"; +static const char stat_217[] PROGMEM = "217"; +static const char stat_218[] PROGMEM = "218"; +static const char stat_219[] PROGMEM = "219"; + +static const char stat_220[] PROGMEM = "Soft limit exceeded"; +static const char stat_221[] PROGMEM = "Soft limit exceeded - X min"; +static const char stat_222[] PROGMEM = "Soft limit exceeded - X max"; +static const char stat_223[] PROGMEM = "Soft limit exceeded - Y min"; +static const char stat_224[] PROGMEM = "Soft limit exceeded - Y max"; +static const char stat_225[] PROGMEM = "Soft limit exceeded - Z min"; +static const char stat_226[] PROGMEM = "Soft limit exceeded - Z max"; +static const char stat_227[] PROGMEM = "Soft limit exceeded - A min"; +static const char stat_228[] PROGMEM = "Soft limit exceeded - A max"; +static const char stat_229[] PROGMEM = "Soft limit exceeded - B min"; +static const char stat_230[] PROGMEM = "Soft limit exceeded - B max"; +static const char stat_231[] PROGMEM = "Soft limit exceeded - C min"; +static const char stat_232[] PROGMEM = "Soft limit exceeded - C max"; +static const char stat_233[] PROGMEM = "233"; +static const char stat_234[] PROGMEM = "234"; +static const char stat_235[] PROGMEM = "235"; +static const char stat_236[] PROGMEM = "236"; +static const char stat_237[] PROGMEM = "237"; +static const char stat_238[] PROGMEM = "238"; +static const char stat_239[] PROGMEM = "239"; + +static const char stat_240[] PROGMEM = "Homing cycle failed"; +static const char stat_241[] PROGMEM = "Homing Error - Bad or no axis specified"; +static const char stat_242[] PROGMEM = "Homing Error - Search velocity is zero"; +static const char stat_243[] PROGMEM = "Homing Error - Latch velocity is zero"; +static const char stat_244[] PROGMEM = "Homing Error - Travel min & max are the same"; +static const char stat_245[] PROGMEM = "Homing Error - Negative latch backoff"; +static const char stat_246[] PROGMEM = "Homing Error - Homing switches misconfigured"; +static const char stat_247[] PROGMEM = "247"; +static const char stat_248[] PROGMEM = "248"; +static const char stat_249[] PROGMEM = "249"; + +static const char stat_250[] PROGMEM = "Probe cycle failed"; +static const char stat_251[] PROGMEM = "Probe endpoint is starting point"; +static const char stat_252[] PROGMEM = "Jogging cycle failed"; + +static const char *const stat_msg[] PROGMEM = { + stat_00, stat_01, stat_02, stat_03, stat_04, stat_05, stat_06, stat_07, stat_08, stat_09, + stat_10, stat_11, stat_12, stat_13, stat_14, stat_15, stat_16, stat_17, stat_18, stat_19, + stat_20, stat_21, stat_22, stat_23, stat_24, stat_25, stat_26, stat_27, stat_28, stat_29, + stat_30, stat_31, stat_32, stat_33, stat_34, stat_35, stat_36, stat_37, stat_38, stat_39, + stat_40, stat_41, stat_42, stat_43, stat_44, stat_45, stat_46, stat_47, stat_48, stat_49, + stat_50, stat_51, stat_52, stat_53, stat_54, stat_55, stat_56, stat_57, stat_58, stat_59, + stat_60, stat_61, stat_62, stat_63, stat_64, stat_65, stat_66, stat_67, stat_68, stat_69, + stat_70, stat_71, stat_72, stat_73, stat_74, stat_75, stat_76, stat_77, stat_78, stat_79, + stat_80, stat_81, stat_82, stat_83, stat_84, stat_85, stat_86, stat_87, stat_88, stat_89, + stat_90, stat_91, stat_92, stat_93, stat_94, stat_95, stat_96, stat_97, stat_98, stat_99, + stat_100, stat_101, stat_102, stat_103, stat_104, stat_105, stat_106, stat_107, stat_108, stat_109, + stat_110, stat_111, stat_112, stat_113, stat_114, stat_115, stat_116, stat_117, stat_118, stat_119, + stat_120, stat_121, stat_122, stat_123, stat_124, stat_125, stat_126, stat_127, stat_128, stat_129, + stat_130, stat_131, stat_132, stat_133, stat_134, stat_135, stat_136, stat_137, stat_138, stat_139, + stat_140, stat_141, stat_142, stat_143, stat_144, stat_145, stat_146, stat_147, stat_148, stat_149, + stat_150, stat_151, stat_152, stat_153, stat_154, stat_155, stat_156, stat_157, stat_158, stat_159, + stat_160, stat_161, stat_162, stat_163, stat_164, stat_165, stat_166, stat_167, stat_168, stat_169, + stat_170, stat_171, stat_172, stat_173, stat_174, stat_175, stat_176, stat_177, stat_178, stat_179, + stat_180, stat_181, stat_182, stat_183, stat_184, stat_185, stat_186, stat_187, stat_188, stat_189, + stat_190, stat_191, stat_192, stat_193, stat_194, stat_195, stat_196, stat_197, stat_198, stat_199, + stat_200, stat_201, stat_202, stat_203, stat_204, stat_205, stat_206, stat_207, stat_208, stat_209, + stat_210, stat_211, stat_212, stat_213, stat_214, stat_215, stat_216, stat_217, stat_218, stat_219, + stat_220, stat_221, stat_222, stat_223, stat_224, stat_225, stat_226, stat_227, stat_228, stat_229, + stat_230, stat_231, stat_232, stat_233, stat_234, stat_235, stat_236, stat_237, stat_238, stat_239, + stat_240, stat_241, stat_242, stat_243, stat_244, stat_245, stat_246, stat_247, stat_248, stat_249, + stat_250, stat_251, stat_252 +}; + + +char *get_status_message(stat_t status) { + return (char *)GET_TEXT_ITEM(stat_msg, status); +} diff --git a/src/stepper.c b/src/stepper.c index a64fc36..9301854 100755 --- a/src/stepper.c +++ b/src/stepper.c @@ -25,8 +25,8 @@ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* This module provides the low-level stepper drivers and some related functions. - * See stepper.h for a detailed explanation of this module. +/* This module provides the low-level stepper drivers and some related functions. + * See stepper.h for a detailed explanation of this module. */ #include "tinyg.h" @@ -47,8 +47,8 @@ static stRunSingleton_t st_run; /**** Setup local functions ****/ -static void _load_move(void); -static void _request_load_move(void); +static void _load_move(); +static void _request_load_move(); // handy macro #define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f) @@ -56,62 +56,62 @@ static void _request_load_move(void); /* * stepper_init() - initialize stepper motor subsystem * - * Notes: - * - This init requires sys_init() to be run beforehand - * - microsteps are setup during config_init() - * - motor polarity is setup during config_init() - * - high level interrupts must be enabled in main() once all inits are complete + * Notes: + * - This init requires sys_init() to be run beforehand + * - microsteps are setup during config_init() + * - motor polarity is setup during config_init() + * - high level interrupts must be enabled in main() once all inits are complete */ -/* NOTE: This is the bare code that the Motate timer calls replace. - * NB: requires: #include +/* NOTE: This is the bare code that the Motate timer calls replace. + * NB: requires: #include * - * REG_TC1_WPMR = 0x54494D00; // enable write to registers - * TC_Configure(TC_BLOCK_DDA, TC_CHANNEL_DDA, TC_CMR_DDA); - * REG_RC_DDA = TC_RC_DDA; // set frequency - * REG_IER_DDA = TC_IER_DDA; // enable interrupts - * NVIC_EnableIRQ(TC_IRQn_DDA); - * pmc_enable_periph_clk(TC_ID_DDA); - * TC_Start(TC_BLOCK_DDA, TC_CHANNEL_DDA); + * REG_TC1_WPMR = 0x54494D00; // enable write to registers + * TC_Configure(TC_BLOCK_DDA, TC_CHANNEL_DDA, TC_CMR_DDA); + * REG_RC_DDA = TC_RC_DDA; // set frequency + * REG_IER_DDA = TC_IER_DDA; // enable interrupts + * NVIC_EnableIRQ(TC_IRQn_DDA); + * pmc_enable_periph_clk(TC_ID_DDA); + * TC_Start(TC_BLOCK_DDA, TC_CHANNEL_DDA); */ void stepper_init() { - memset(&st_run, 0, sizeof(st_run)); // clear all values, pointers and status - stepper_init_assertions(); + memset(&st_run, 0, sizeof(st_run)); // clear all values, pointers and status + stepper_init_assertions(); // 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 - for (uint8_t i=0; iDIR = MOTOR_PORT_DIR_gm; // sets outputs for motors & GPIO1, and GPIO2 inputs - hw.st_port[i]->OUT = MOTOR_ENABLE_BIT_bm;// zero port bits AND disable motor - } - // 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 - - // setup DWELL timer - TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // turn timer off - TIMER_DWELL.CTRLB = STEP_TIMER_WGMODE; // waveform mode - TIMER_DWELL.INTCTRLA = TIMER_DWELL_INTLVL; // interrupt mode - - // setup software interrupt load timer - TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // turn timer off - TIMER_LOAD.CTRLB = LOAD_TIMER_WGMODE; // waveform mode - TIMER_LOAD.INTCTRLA = TIMER_LOAD_INTLVL; // interrupt mode - TIMER_LOAD.PER = LOAD_TIMER_PERIOD; // set period - - // setup software interrupt exec timer - TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // turn timer off - TIMER_EXEC.CTRLB = EXEC_TIMER_WGMODE; // waveform mode - TIMER_EXEC.INTCTRLA = TIMER_EXEC_INTLVL; // interrupt mode - TIMER_EXEC.PER = EXEC_TIMER_PERIOD; // set period - - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; - st_reset(); // reset steppers to known state + PORTCFG.VPCTRLA = PORTCFG_VP0MAP_PORT_MOTOR_1_gc | PORTCFG_VP1MAP_PORT_MOTOR_2_gc; + PORTCFG.VPCTRLB = PORTCFG_VP2MAP_PORT_MOTOR_3_gc | PORTCFG_VP3MAP_PORT_MOTOR_4_gc; + + // setup ports and data structures + for (uint8_t i=0; iDIR = MOTOR_PORT_DIR_gm; // sets outputs for motors & GPIO1, and GPIO2 inputs + hw.st_port[i]->OUT = MOTOR_ENABLE_BIT_bm;// zero port bits AND disable motor + } + // 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 + + // setup DWELL timer + TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // turn timer off + TIMER_DWELL.CTRLB = STEP_TIMER_WGMODE; // waveform mode + TIMER_DWELL.INTCTRLA = TIMER_DWELL_INTLVL; // interrupt mode + + // setup software interrupt load timer + TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // turn timer off + TIMER_LOAD.CTRLB = LOAD_TIMER_WGMODE; // waveform mode + TIMER_LOAD.INTCTRLA = TIMER_LOAD_INTLVL; // interrupt mode + TIMER_LOAD.PER = LOAD_TIMER_PERIOD; // set period + + // setup software interrupt exec timer + TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // turn timer off + TIMER_EXEC.CTRLB = EXEC_TIMER_WGMODE; // waveform mode + TIMER_EXEC.INTCTRLA = TIMER_EXEC_INTLVL; // interrupt mode + TIMER_EXEC.PER = EXEC_TIMER_PERIOD; // set period + + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; + st_reset(); // reset steppers to known state } /* @@ -121,35 +121,35 @@ void stepper_init() void stepper_init_assertions() { - st_run.magic_end = MAGICNUM; - st_run.magic_start = MAGICNUM; - st_pre.magic_end = MAGICNUM; - st_pre.magic_start = MAGICNUM; + st_run.magic_end = MAGICNUM; + st_run.magic_start = MAGICNUM; + st_pre.magic_end = MAGICNUM; + st_pre.magic_start = MAGICNUM; } stat_t stepper_test_assertions() { - if (st_run.magic_end != MAGICNUM) return (STAT_STEPPER_ASSERTION_FAILURE); - if (st_run.magic_start != MAGICNUM) return (STAT_STEPPER_ASSERTION_FAILURE); - if (st_pre.magic_end != MAGICNUM) return (STAT_STEPPER_ASSERTION_FAILURE); - if (st_pre.magic_start != MAGICNUM) return (STAT_STEPPER_ASSERTION_FAILURE); - return (STAT_OK); + if (st_run.magic_end != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; + if (st_run.magic_start != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; + if (st_pre.magic_end != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; + if (st_pre.magic_start != MAGICNUM) return STAT_STEPPER_ASSERTION_FAILURE; + return STAT_OK; } /* * st_runtime_isbusy() - return TRUE if runtime is busy: * - * Busy conditions: - * - motors are running - * - dwell is running + * Busy conditions: + * - motors are running + * - dwell is running */ uint8_t st_runtime_isbusy() { - if (st_run.dda_ticks_downcount == 0) { - return (false); - } - return (true); + if (st_run.dda_ticks_downcount == 0) { + return false; + } + return true; } /* @@ -158,141 +158,141 @@ uint8_t st_runtime_isbusy() void st_reset() { - for (uint8_t motor=0; motor st_run.mot[m].power_systick ) { - st_run.mot[m].power_state = MOTOR_IDLE; - _deenergize_motor(m); - sr_request_status_report(SR_TIMED_REQUEST); // request a status report when motors shut down - } - } - } - return (STAT_OK); +stat_t st_motor_power_callback() // called by controller +{ + // manage power for each motor individually + for (uint8_t m = MOTOR_1; 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 = SysTickTimer_getValue() + + (st_cfg.motor_power_timeout * 1000); + } + + // do not process countdown if in a feedhold + if (cm_get_combined_state() == COMBINED_HOLD) { + continue; + } + + // do not process countdown if in a feedhold + if (cm_get_combined_state() == COMBINED_HOLD) { + continue; + } + + // run the countdown if you are in a countdown + if (st_run.mot[m].power_state == MOTOR_POWER_TIMEOUT_COUNTDOWN) { + if (SysTickTimer_getValue() > st_run.mot[m].power_systick ) { + st_run.mot[m].power_state = MOTOR_IDLE; + _deenergize_motor(m); + sr_request_status_report(SR_TIMED_REQUEST); // request a status report when motors shut down + } + } + } + return STAT_OK; } @@ -305,42 +305,42 @@ stat_t st_motor_power_callback() // called by controller */ /* - * Uses direct struct addresses and literal values for hardware devices - it's faster than - * using indexed timer and port accesses. I checked. Even when -0s or -03 is used. + * Uses direct struct addresses and literal values for hardware devices - it's faster than + * using indexed timer and port accesses. I checked. Even when -0s or -03 is used. */ ISR(TIMER_DDA_ISR_vect) { - if ((st_run.mot[MOTOR_1].substep_accumulator += st_run.mot[MOTOR_1].substep_increment) > 0) { - PORT_MOTOR_1_VPORT.OUT |= STEP_BIT_bm; // turn step bit on - st_run.mot[MOTOR_1].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_1); - } - if ((st_run.mot[MOTOR_2].substep_accumulator += st_run.mot[MOTOR_2].substep_increment) > 0) { - PORT_MOTOR_2_VPORT.OUT |= STEP_BIT_bm; - st_run.mot[MOTOR_2].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_2); - } - if ((st_run.mot[MOTOR_3].substep_accumulator += st_run.mot[MOTOR_3].substep_increment) > 0) { - PORT_MOTOR_3_VPORT.OUT |= STEP_BIT_bm; - st_run.mot[MOTOR_3].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_3); - } - if ((st_run.mot[MOTOR_4].substep_accumulator += st_run.mot[MOTOR_4].substep_increment) > 0) { - PORT_MOTOR_4_VPORT.OUT |= STEP_BIT_bm; - st_run.mot[MOTOR_4].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_4); - } - - // pulse stretching for using external drivers.- turn step bits off - PORT_MOTOR_1_VPORT.OUT &= ~STEP_BIT_bm; // ~ 5 uSec pulse width - PORT_MOTOR_2_VPORT.OUT &= ~STEP_BIT_bm; // ~ 4 uSec - PORT_MOTOR_3_VPORT.OUT &= ~STEP_BIT_bm; // ~ 3 uSec - PORT_MOTOR_4_VPORT.OUT &= ~STEP_BIT_bm; // ~ 2 uSec - - if (--st_run.dda_ticks_downcount != 0) return; - - TIMER_DDA.CTRLA = STEP_TIMER_DISABLE; // disable DDA timer - _load_move(); // load the next move + if ((st_run.mot[MOTOR_1].substep_accumulator += st_run.mot[MOTOR_1].substep_increment) > 0) { + PORT_MOTOR_1_VPORT.OUT |= STEP_BIT_bm; // turn step bit on + st_run.mot[MOTOR_1].substep_accumulator -= st_run.dda_ticks_X_substeps; + INCREMENT_ENCODER(MOTOR_1); + } + if ((st_run.mot[MOTOR_2].substep_accumulator += st_run.mot[MOTOR_2].substep_increment) > 0) { + PORT_MOTOR_2_VPORT.OUT |= STEP_BIT_bm; + st_run.mot[MOTOR_2].substep_accumulator -= st_run.dda_ticks_X_substeps; + INCREMENT_ENCODER(MOTOR_2); + } + if ((st_run.mot[MOTOR_3].substep_accumulator += st_run.mot[MOTOR_3].substep_increment) > 0) { + PORT_MOTOR_3_VPORT.OUT |= STEP_BIT_bm; + st_run.mot[MOTOR_3].substep_accumulator -= st_run.dda_ticks_X_substeps; + INCREMENT_ENCODER(MOTOR_3); + } + if ((st_run.mot[MOTOR_4].substep_accumulator += st_run.mot[MOTOR_4].substep_increment) > 0) { + PORT_MOTOR_4_VPORT.OUT |= STEP_BIT_bm; + st_run.mot[MOTOR_4].substep_accumulator -= st_run.dda_ticks_X_substeps; + INCREMENT_ENCODER(MOTOR_4); + } + + // pulse stretching for using external drivers.- turn step bits off + PORT_MOTOR_1_VPORT.OUT &= ~STEP_BIT_bm; // ~ 5 uSec pulse width + PORT_MOTOR_2_VPORT.OUT &= ~STEP_BIT_bm; // ~ 4 uSec + PORT_MOTOR_3_VPORT.OUT &= ~STEP_BIT_bm; // ~ 3 uSec + PORT_MOTOR_4_VPORT.OUT &= ~STEP_BIT_bm; // ~ 2 uSec + + if (--st_run.dda_ticks_downcount != 0) return; + + TIMER_DDA.CTRLA = STEP_TIMER_DISABLE; // disable DDA timer + _load_move(); // load the next move } @@ -348,390 +348,390 @@ ISR(TIMER_DDA_ISR_vect) * ISR - DDA timer interrupt routine - service ticks from DDA timer */ -ISR(TIMER_DWELL_ISR_vect) { // DWELL timer interrupt - if (--st_run.dda_ticks_downcount == 0) { - TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // disable DWELL timer - _load_move(); - } +ISR(TIMER_DWELL_ISR_vect) { // DWELL timer interrupt + if (--st_run.dda_ticks_downcount == 0) { + TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // disable DWELL timer + _load_move(); + } } /**************************************************************************************** - * Exec sequencing code - computes and prepares next load segment - * st_request_exec_move() - SW interrupt to request to execute a move - * exec_timer interrupt - interrupt handler for calling exec function + * Exec sequencing code - computes and prepares next load segment + * st_request_exec_move() - SW interrupt to request to execute a move + * exec_timer interrupt - interrupt handler for calling exec function */ void st_request_exec_move() { - if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) {// bother interrupting - TIMER_EXEC.PER = EXEC_TIMER_PERIOD; - TIMER_EXEC.CTRLA = EXEC_TIMER_ENABLE; // trigger a LO interrupt - } + if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) {// bother interrupting + TIMER_EXEC.PER = EXEC_TIMER_PERIOD; + TIMER_EXEC.CTRLA = EXEC_TIMER_ENABLE; // trigger a LO interrupt + } } -ISR(TIMER_EXEC_ISR_vect) { // exec move SW interrupt - TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // disable SW interrupt timer +ISR(TIMER_EXEC_ISR_vect) { // exec move SW interrupt + TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // disable SW interrupt timer - // exec_move - if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) { - if (mp_exec_move() != STAT_NOOP) { - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // flip it back - _request_load_move(); - } - } + // exec_move + if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) { + if (mp_exec_move() != STAT_NOOP) { + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // flip it back + _request_load_move(); + } + } } /**************************************************************************************** * Loader sequencing code * st_request_load_move() - fires a software interrupt (timer) to request to load a move - * load_move interrupt - interrupt handler for running the loader + * load_move interrupt - interrupt handler for running the loader * - * _load_move() can only be called be called from an ISR at the same or higher level as - * the DDA or dwell ISR. A software interrupt has been provided to allow a non-ISR to - * request a load (see st_request_load_move()) + * _load_move() can only be called be called from an ISR at the same or higher level as + * the DDA or dwell ISR. A software interrupt has been provided to allow a non-ISR to + * request a load (see st_request_load_move()) */ static void _request_load_move() { - if (st_runtime_isbusy()) { - return; // don't request a load if the runtime is busy - } - if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_LOADER) { // bother interrupting - TIMER_LOAD.PER = LOAD_TIMER_PERIOD; - TIMER_LOAD.CTRLA = LOAD_TIMER_ENABLE; // trigger a HI interrupt - } + if (st_runtime_isbusy()) { + return; // don't request a load if the runtime is busy + } + if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_LOADER) { // bother interrupting + TIMER_LOAD.PER = LOAD_TIMER_PERIOD; + TIMER_LOAD.CTRLA = LOAD_TIMER_ENABLE; // trigger a HI interrupt + } } -ISR(TIMER_LOAD_ISR_vect) { // load steppers SW interrupt - TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // disable SW interrupt timer - _load_move(); +ISR(TIMER_LOAD_ISR_vect) { // load steppers SW interrupt + TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // disable SW interrupt timer + _load_move(); } /**************************************************************************************** * _load_move() - Dequeue move and load into stepper struct * - * This routine can only be called be called from an ISR at the same or - * higher level as the DDA or dwell ISR. A software interrupt has been - * provided to allow a non-ISR to request a load (see st_request_load_move()) + * This routine can only be called be called from an ISR at the same or + * higher level as the DDA or dwell ISR. A software interrupt has been + * provided to allow a non-ISR to request a load (see st_request_load_move()) * - * In aline() code: - * - All axes must set steps and compensate for out-of-range pulse phasing. - * - If axis has 0 steps the direction setting can be omitted - * - If axis has 0 steps the motor must not be enabled to support power mode = 1 + * In aline() code: + * - All axes must set steps and compensate for out-of-range pulse phasing. + * - If axis has 0 steps the direction setting can be omitted + * - If axis has 0 steps the motor must not be enabled to support power mode = 1 */ static void _load_move() { - // Be aware that dda_ticks_downcount must equal zero for the loader to run. - // So the initial load must also have this set to zero as part of initialization - if (st_runtime_isbusy()) { - return; // exit if the runtime is busy - } - if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) { // if there are no moves to load... + // 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) { // if there are no moves to load... return; - } - // handle aline loads first (most common case) - if (st_pre.move_type == MOVE_TYPE_ALINE) { - - //**** setup the new segment **** - - st_run.dda_ticks_downcount = st_pre.dda_ticks; - st_run.dda_ticks_X_substeps = st_pre.dda_ticks_X_substeps; - - //**** MOTOR_1 LOAD **** - - // These sections are somewhat optimized for execution speed. The whole load operation - // is supposed to take < 10 uSec (Xmega). Be careful if you mess with this. - - // the following if() statement sets the runtime substep increment value or zeroes it - if ((st_run.mot[MOTOR_1].substep_increment = st_pre.mot[MOTOR_1].substep_increment) != 0) { - - // NB: If motor has 0 steps the following is all skipped. This ensures that state comparisons - // always operate on the last segment actually run by this motor, regardless of how many - // segments it may have been inactive in between. - - // Apply accumulator correction if the time base has changed since previous segment - if (st_pre.mot[MOTOR_1].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_1].accumulator_correction_flag = false; - st_run.mot[MOTOR_1].substep_accumulator *= st_pre.mot[MOTOR_1].accumulator_correction; - } - - // Detect direction change and if so: - // - Set the direction bit in hardware. - // - Compensate for direction change by flipping substep accumulator value about its midpoint. - - if (st_pre.mot[MOTOR_1].direction != st_pre.mot[MOTOR_1].prev_direction) { - st_pre.mot[MOTOR_1].prev_direction = st_pre.mot[MOTOR_1].direction; - st_run.mot[MOTOR_1].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_1].substep_accumulator); - if (st_pre.mot[MOTOR_1].direction == DIRECTION_CW) - PORT_MOTOR_1_VPORT.OUT &= ~DIRECTION_BIT_bm; else - PORT_MOTOR_1_VPORT.OUT |= DIRECTION_BIT_bm; - } - SET_ENCODER_STEP_SIGN(MOTOR_1, st_pre.mot[MOTOR_1].step_sign); - - // Enable the stepper and start motor power management - if (st_cfg.mot[MOTOR_1].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_1_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor - st_run.mot[MOTOR_1].power_state = MOTOR_POWER_TIMEOUT_START;// set power management state - } - - } else { // Motor has 0 steps; might need to energize motor for power mode processing - if (st_cfg.mot[MOTOR_1].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_1_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor - st_run.mot[MOTOR_1].power_state = MOTOR_POWER_TIMEOUT_START; - } - } - // accumulate counted steps to the step position and zero out counted steps for the segment currently being loaded - ACCUMULATE_ENCODER(MOTOR_1); - -#if (MOTORS >= 2) //**** MOTOR_2 LOAD **** - if ((st_run.mot[MOTOR_2].substep_increment = st_pre.mot[MOTOR_2].substep_increment) != 0) { - if (st_pre.mot[MOTOR_2].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_2].accumulator_correction_flag = false; - st_run.mot[MOTOR_2].substep_accumulator *= st_pre.mot[MOTOR_2].accumulator_correction; - } - if (st_pre.mot[MOTOR_2].direction != st_pre.mot[MOTOR_2].prev_direction) { - st_pre.mot[MOTOR_2].prev_direction = st_pre.mot[MOTOR_2].direction; - st_run.mot[MOTOR_2].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_2].substep_accumulator); - if (st_pre.mot[MOTOR_2].direction == DIRECTION_CW) - PORT_MOTOR_2_VPORT.OUT &= ~DIRECTION_BIT_bm; else - PORT_MOTOR_2_VPORT.OUT |= DIRECTION_BIT_bm; - } - SET_ENCODER_STEP_SIGN(MOTOR_2, st_pre.mot[MOTOR_2].step_sign); - if (st_cfg.mot[MOTOR_2].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_2_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_2].power_state = MOTOR_POWER_TIMEOUT_START; - } - } else { - if (st_cfg.mot[MOTOR_2].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_2_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_2].power_state = MOTOR_POWER_TIMEOUT_START; - } - } - ACCUMULATE_ENCODER(MOTOR_2); + } + // handle aline loads first (most common case) + if (st_pre.move_type == MOVE_TYPE_ALINE) { + + //**** setup the new segment **** + + st_run.dda_ticks_downcount = st_pre.dda_ticks; + st_run.dda_ticks_X_substeps = st_pre.dda_ticks_X_substeps; + + //**** MOTOR_1 LOAD **** + + // These sections are somewhat optimized for execution speed. The whole load operation + // is supposed to take < 10 uSec (Xmega). Be careful if you mess with this. + + // the following if() statement sets the runtime substep increment value or zeroes it + if ((st_run.mot[MOTOR_1].substep_increment = st_pre.mot[MOTOR_1].substep_increment) != 0) { + + // NB: If motor has 0 steps the following is all skipped. This ensures that state comparisons + // always operate on the last segment actually run by this motor, regardless of how many + // segments it may have been inactive in between. + + // Apply accumulator correction if the time base has changed since previous segment + if (st_pre.mot[MOTOR_1].accumulator_correction_flag == true) { + st_pre.mot[MOTOR_1].accumulator_correction_flag = false; + st_run.mot[MOTOR_1].substep_accumulator *= st_pre.mot[MOTOR_1].accumulator_correction; + } + + // Detect direction change and if so: + // - Set the direction bit in hardware. + // - Compensate for direction change by flipping substep accumulator value about its midpoint. + + if (st_pre.mot[MOTOR_1].direction != st_pre.mot[MOTOR_1].prev_direction) { + st_pre.mot[MOTOR_1].prev_direction = st_pre.mot[MOTOR_1].direction; + st_run.mot[MOTOR_1].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_1].substep_accumulator); + if (st_pre.mot[MOTOR_1].direction == DIRECTION_CW) + PORT_MOTOR_1_VPORT.OUT &= ~DIRECTION_BIT_bm; else + PORT_MOTOR_1_VPORT.OUT |= DIRECTION_BIT_bm; + } + SET_ENCODER_STEP_SIGN(MOTOR_1, st_pre.mot[MOTOR_1].step_sign); + + // Enable the stepper and start motor power management + if (st_cfg.mot[MOTOR_1].power_mode != MOTOR_DISABLED) { + PORT_MOTOR_1_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor + st_run.mot[MOTOR_1].power_state = MOTOR_POWER_TIMEOUT_START;// set power management state + } + + } else { // Motor has 0 steps; might need to energize motor for power mode processing + if (st_cfg.mot[MOTOR_1].power_mode == MOTOR_POWERED_IN_CYCLE) { + PORT_MOTOR_1_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor + st_run.mot[MOTOR_1].power_state = MOTOR_POWER_TIMEOUT_START; + } + } + // accumulate counted steps to the step position and zero out counted steps for the segment currently being loaded + ACCUMULATE_ENCODER(MOTOR_1); + +#if (MOTORS >= 2) //**** MOTOR_2 LOAD **** + if ((st_run.mot[MOTOR_2].substep_increment = st_pre.mot[MOTOR_2].substep_increment) != 0) { + if (st_pre.mot[MOTOR_2].accumulator_correction_flag == true) { + st_pre.mot[MOTOR_2].accumulator_correction_flag = false; + st_run.mot[MOTOR_2].substep_accumulator *= st_pre.mot[MOTOR_2].accumulator_correction; + } + if (st_pre.mot[MOTOR_2].direction != st_pre.mot[MOTOR_2].prev_direction) { + st_pre.mot[MOTOR_2].prev_direction = st_pre.mot[MOTOR_2].direction; + st_run.mot[MOTOR_2].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_2].substep_accumulator); + if (st_pre.mot[MOTOR_2].direction == DIRECTION_CW) + PORT_MOTOR_2_VPORT.OUT &= ~DIRECTION_BIT_bm; else + PORT_MOTOR_2_VPORT.OUT |= DIRECTION_BIT_bm; + } + SET_ENCODER_STEP_SIGN(MOTOR_2, st_pre.mot[MOTOR_2].step_sign); + if (st_cfg.mot[MOTOR_2].power_mode != MOTOR_DISABLED) { + PORT_MOTOR_2_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_2].power_state = MOTOR_POWER_TIMEOUT_START; + } + } else { + if (st_cfg.mot[MOTOR_2].power_mode == MOTOR_POWERED_IN_CYCLE) { + PORT_MOTOR_2_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_2].power_state = MOTOR_POWER_TIMEOUT_START; + } + } + ACCUMULATE_ENCODER(MOTOR_2); #endif -#if (MOTORS >= 3) //**** MOTOR_3 LOAD **** - if ((st_run.mot[MOTOR_3].substep_increment = st_pre.mot[MOTOR_3].substep_increment) != 0) { - if (st_pre.mot[MOTOR_3].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_3].accumulator_correction_flag = false; - st_run.mot[MOTOR_3].substep_accumulator *= st_pre.mot[MOTOR_3].accumulator_correction; - } - if (st_pre.mot[MOTOR_3].direction != st_pre.mot[MOTOR_3].prev_direction) { - st_pre.mot[MOTOR_3].prev_direction = st_pre.mot[MOTOR_3].direction; - st_run.mot[MOTOR_3].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_3].substep_accumulator); - if (st_pre.mot[MOTOR_3].direction == DIRECTION_CW) - PORT_MOTOR_3_VPORT.OUT &= ~DIRECTION_BIT_bm; else - PORT_MOTOR_3_VPORT.OUT |= DIRECTION_BIT_bm; - } - SET_ENCODER_STEP_SIGN(MOTOR_3, st_pre.mot[MOTOR_3].step_sign); - if (st_cfg.mot[MOTOR_3].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_3_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_3].power_state = MOTOR_POWER_TIMEOUT_START; - } - } else { - if (st_cfg.mot[MOTOR_3].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_3_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_3].power_state = MOTOR_POWER_TIMEOUT_START; - } - } - ACCUMULATE_ENCODER(MOTOR_3); +#if (MOTORS >= 3) //**** MOTOR_3 LOAD **** + if ((st_run.mot[MOTOR_3].substep_increment = st_pre.mot[MOTOR_3].substep_increment) != 0) { + if (st_pre.mot[MOTOR_3].accumulator_correction_flag == true) { + st_pre.mot[MOTOR_3].accumulator_correction_flag = false; + st_run.mot[MOTOR_3].substep_accumulator *= st_pre.mot[MOTOR_3].accumulator_correction; + } + if (st_pre.mot[MOTOR_3].direction != st_pre.mot[MOTOR_3].prev_direction) { + st_pre.mot[MOTOR_3].prev_direction = st_pre.mot[MOTOR_3].direction; + st_run.mot[MOTOR_3].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_3].substep_accumulator); + if (st_pre.mot[MOTOR_3].direction == DIRECTION_CW) + PORT_MOTOR_3_VPORT.OUT &= ~DIRECTION_BIT_bm; else + PORT_MOTOR_3_VPORT.OUT |= DIRECTION_BIT_bm; + } + SET_ENCODER_STEP_SIGN(MOTOR_3, st_pre.mot[MOTOR_3].step_sign); + if (st_cfg.mot[MOTOR_3].power_mode != MOTOR_DISABLED) { + PORT_MOTOR_3_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_3].power_state = MOTOR_POWER_TIMEOUT_START; + } + } else { + if (st_cfg.mot[MOTOR_3].power_mode == MOTOR_POWERED_IN_CYCLE) { + PORT_MOTOR_3_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_3].power_state = MOTOR_POWER_TIMEOUT_START; + } + } + ACCUMULATE_ENCODER(MOTOR_3); #endif #if (MOTORS >= 4) //**** MOTOR_4 LOAD **** - if ((st_run.mot[MOTOR_4].substep_increment = st_pre.mot[MOTOR_4].substep_increment) != 0) { - if (st_pre.mot[MOTOR_4].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_4].accumulator_correction_flag = false; - st_run.mot[MOTOR_4].substep_accumulator *= st_pre.mot[MOTOR_4].accumulator_correction; - } - if (st_pre.mot[MOTOR_4].direction != st_pre.mot[MOTOR_4].prev_direction) { - st_pre.mot[MOTOR_4].prev_direction = st_pre.mot[MOTOR_4].direction; - st_run.mot[MOTOR_4].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_4].substep_accumulator); - if (st_pre.mot[MOTOR_4].direction == DIRECTION_CW) - PORT_MOTOR_4_VPORT.OUT &= ~DIRECTION_BIT_bm; else - PORT_MOTOR_4_VPORT.OUT |= DIRECTION_BIT_bm; - } - SET_ENCODER_STEP_SIGN(MOTOR_4, st_pre.mot[MOTOR_4].step_sign); - if (st_cfg.mot[MOTOR_4].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_4_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_4].power_state = MOTOR_POWER_TIMEOUT_START; - } - } else { - if (st_cfg.mot[MOTOR_4].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_4_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_4].power_state = MOTOR_POWER_TIMEOUT_START; - } - } - ACCUMULATE_ENCODER(MOTOR_4); + if ((st_run.mot[MOTOR_4].substep_increment = st_pre.mot[MOTOR_4].substep_increment) != 0) { + if (st_pre.mot[MOTOR_4].accumulator_correction_flag == true) { + st_pre.mot[MOTOR_4].accumulator_correction_flag = false; + st_run.mot[MOTOR_4].substep_accumulator *= st_pre.mot[MOTOR_4].accumulator_correction; + } + if (st_pre.mot[MOTOR_4].direction != st_pre.mot[MOTOR_4].prev_direction) { + st_pre.mot[MOTOR_4].prev_direction = st_pre.mot[MOTOR_4].direction; + st_run.mot[MOTOR_4].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_4].substep_accumulator); + if (st_pre.mot[MOTOR_4].direction == DIRECTION_CW) + PORT_MOTOR_4_VPORT.OUT &= ~DIRECTION_BIT_bm; else + PORT_MOTOR_4_VPORT.OUT |= DIRECTION_BIT_bm; + } + SET_ENCODER_STEP_SIGN(MOTOR_4, st_pre.mot[MOTOR_4].step_sign); + if (st_cfg.mot[MOTOR_4].power_mode != MOTOR_DISABLED) { + PORT_MOTOR_4_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_4].power_state = MOTOR_POWER_TIMEOUT_START; + } + } else { + if (st_cfg.mot[MOTOR_4].power_mode == MOTOR_POWERED_IN_CYCLE) { + PORT_MOTOR_4_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_4].power_state = MOTOR_POWER_TIMEOUT_START; + } + } + ACCUMULATE_ENCODER(MOTOR_4); #endif -#if (MOTORS >= 5) //**** MOTOR_5 LOAD **** - if ((st_run.mot[MOTOR_5].substep_increment = st_pre.mot[MOTOR_5].substep_increment) != 0) { - if (st_pre.mot[MOTOR_5].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_5].accumulator_correction_flag = false; - st_run.mot[MOTOR_5].substep_accumulator *= st_pre.mot[MOTOR_5].accumulator_correction; - } - if (st_pre.mot[MOTOR_5].direction != st_pre.mot[MOTOR_5].prev_direction) { - st_pre.mot[MOTOR_5].prev_direction = st_pre.mot[MOTOR_5].direction; - st_run.mot[MOTOR_5].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_5].substep_accumulator); - if (st_pre.mot[MOTOR_5].direction == DIRECTION_CW) - PORT_MOTOR_5_VPORT.OUT &= ~DIRECTION_BIT_bm; else - PORT_MOTOR_5_VPORT.OUT |= DIRECTION_BIT_bm; - } - PORT_MOTOR_5_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_5].power_state = MOTOR_POWER_TIMEOUT_START; - SET_ENCODER_STEP_SIGN(MOTOR_5, st_pre.mot[MOTOR_5].step_sign); - } else { - if (st_cfg.mot[MOTOR_5].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_5_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_5].power_state = MOTOR_POWER_TIMEOUT_START; - } - } - ACCUMULATE_ENCODER(MOTOR_5); +#if (MOTORS >= 5) //**** MOTOR_5 LOAD **** + if ((st_run.mot[MOTOR_5].substep_increment = st_pre.mot[MOTOR_5].substep_increment) != 0) { + if (st_pre.mot[MOTOR_5].accumulator_correction_flag == true) { + st_pre.mot[MOTOR_5].accumulator_correction_flag = false; + st_run.mot[MOTOR_5].substep_accumulator *= st_pre.mot[MOTOR_5].accumulator_correction; + } + if (st_pre.mot[MOTOR_5].direction != st_pre.mot[MOTOR_5].prev_direction) { + st_pre.mot[MOTOR_5].prev_direction = st_pre.mot[MOTOR_5].direction; + st_run.mot[MOTOR_5].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_5].substep_accumulator); + if (st_pre.mot[MOTOR_5].direction == DIRECTION_CW) + PORT_MOTOR_5_VPORT.OUT &= ~DIRECTION_BIT_bm; else + PORT_MOTOR_5_VPORT.OUT |= DIRECTION_BIT_bm; + } + PORT_MOTOR_5_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_5].power_state = MOTOR_POWER_TIMEOUT_START; + SET_ENCODER_STEP_SIGN(MOTOR_5, st_pre.mot[MOTOR_5].step_sign); + } else { + if (st_cfg.mot[MOTOR_5].power_mode == MOTOR_POWERED_IN_CYCLE) { + PORT_MOTOR_5_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_5].power_state = MOTOR_POWER_TIMEOUT_START; + } + } + ACCUMULATE_ENCODER(MOTOR_5); #endif -#if (MOTORS >= 6) //**** MOTOR_6 LOAD **** - if ((st_run.mot[MOTOR_6].substep_increment = st_pre.mot[MOTOR_6].substep_increment) != 0) { - if (st_pre.mot[MOTOR_6].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_6].accumulator_correction_flag = false; - st_run.mot[MOTOR_6].substep_accumulator *= st_pre.mot[MOTOR_6].accumulator_correction; - } - if (st_pre.mot[MOTOR_6].direction != st_pre.mot[MOTOR_6].prev_direction) { - st_pre.mot[MOTOR_6].prev_direction = st_pre.mot[MOTOR_6].direction; - st_run.mot[MOTOR_6].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_6].substep_accumulator); - if (st_pre.mot[MOTOR_6].direction == DIRECTION_CW) - PORT_MOTOR_6_VPORT.OUT &= ~DIRECTION_BIT_bm; else - PORT_MOTOR_6_VPORT.OUT |= DIRECTION_BIT_bm; - } - PORT_MOTOR_6_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_6].power_state = MOTOR_POWER_TIMEOUT_START; - SET_ENCODER_STEP_SIGN(MOTOR_6, st_pre.mot[MOTOR_6].step_sign); - } else { - if (st_cfg.mot[MOTOR_6].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_6_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_6].power_state = MOTOR_POWER_TIMEOUT_START; - } - } - ACCUMULATE_ENCODER(MOTOR_6); +#if (MOTORS >= 6) //**** MOTOR_6 LOAD **** + if ((st_run.mot[MOTOR_6].substep_increment = st_pre.mot[MOTOR_6].substep_increment) != 0) { + if (st_pre.mot[MOTOR_6].accumulator_correction_flag == true) { + st_pre.mot[MOTOR_6].accumulator_correction_flag = false; + st_run.mot[MOTOR_6].substep_accumulator *= st_pre.mot[MOTOR_6].accumulator_correction; + } + if (st_pre.mot[MOTOR_6].direction != st_pre.mot[MOTOR_6].prev_direction) { + st_pre.mot[MOTOR_6].prev_direction = st_pre.mot[MOTOR_6].direction; + st_run.mot[MOTOR_6].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_6].substep_accumulator); + if (st_pre.mot[MOTOR_6].direction == DIRECTION_CW) + PORT_MOTOR_6_VPORT.OUT &= ~DIRECTION_BIT_bm; else + PORT_MOTOR_6_VPORT.OUT |= DIRECTION_BIT_bm; + } + PORT_MOTOR_6_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_6].power_state = MOTOR_POWER_TIMEOUT_START; + SET_ENCODER_STEP_SIGN(MOTOR_6, st_pre.mot[MOTOR_6].step_sign); + } else { + if (st_cfg.mot[MOTOR_6].power_mode == MOTOR_POWERED_IN_CYCLE) { + PORT_MOTOR_6_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; + st_run.mot[MOTOR_6].power_state = MOTOR_POWER_TIMEOUT_START; + } + } + ACCUMULATE_ENCODER(MOTOR_6); #endif - //**** do this last **** + //**** do this last **** - TIMER_DDA.PER = st_pre.dda_period; - TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the DDA timer + TIMER_DDA.PER = st_pre.dda_period; + TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the DDA timer - // handle dwells - } else if (st_pre.move_type == MOVE_TYPE_DWELL) { - st_run.dda_ticks_downcount = st_pre.dda_ticks; - TIMER_DWELL.PER = st_pre.dda_period; // load dwell timer period - TIMER_DWELL.CTRLA = STEP_TIMER_ENABLE; // enable the dwell timer + // handle dwells + } else if (st_pre.move_type == MOVE_TYPE_DWELL) { + st_run.dda_ticks_downcount = st_pre.dda_ticks; + TIMER_DWELL.PER = st_pre.dda_period; // load dwell timer period + TIMER_DWELL.CTRLA = STEP_TIMER_ENABLE; // enable the dwell timer - // handle synchronous commands - } else if (st_pre.move_type == MOVE_TYPE_COMMAND) { - mp_runtime_command(st_pre.bf); - } + // handle synchronous commands + } else if (st_pre.move_type == MOVE_TYPE_COMMAND) { + mp_runtime_command(st_pre.bf); + } - // all other cases drop to here (e.g. Null moves after Mcodes skip to here) - st_pre.move_type = MOVE_TYPE_NULL; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // we are done with the prep buffer - flip the flag back - st_request_exec_move(); // exec and prep next move + // all other cases drop to here (e.g. Null moves after Mcodes skip to here) + st_pre.move_type = MOVE_TYPE_0; + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // we are done with the prep buffer - flip the flag back + st_request_exec_move(); // exec and prep next move } /*********************************************************************************** * st_prep_line() - Prepare the next move for the loader * - * This function does the math on the next pulse segment and gets it ready for - * the loader. It deals with all the DDA optimizations and timer setups so that - * loading can be performed as rapidly as possible. It works in joint space - * (motors) and it works in steps, not length units. All args are provided as - * floats and converted to their appropriate integer types for the loader. + * This function does the math on the next pulse segment and gets it ready for + * the loader. It deals with all the DDA optimizations and timer setups so that + * loading can be performed as rapidly as possible. It works in joint space + * (motors) and it works in steps, not length units. All args are provided as + * floats and converted to their appropriate integer types for the loader. * * Args: - * - travel_steps[] are signed relative motion in steps for each motor. Steps are - * floats that typically have fractional values (fractional steps). The sign - * indicates direction. Motors that are not in the move should be 0 steps on input. + * - travel_steps[] are signed relative motion in steps for each motor. Steps are + * floats that typically have fractional values (fractional steps). The sign + * indicates direction. Motors that are not in the move should be 0 steps on input. * - * - following_error[] is a vector of measured errors to the step count. Used for correction. + * - following_error[] is a vector of measured errors to the step count. Used for correction. * - * - segment_time - how many minutes the segment should run. If timing is not - * 100% accurate this will affect the move velocity, but not the distance traveled. + * - segment_time - how many minutes the segment should run. If timing is not + * 100% accurate this will affect the move velocity, but not the distance traveled. * * NOTE: Many of the expressions are sensitive to casting and execution order to avoid long-term - * accuracy errors due to floating point round off. One earlier failed attempt was: - * dda_ticks_X_substeps = (int32_t)((microseconds/1000000) * f_dda * dda_substeps); + * accuracy errors due to floating point round off. One earlier failed attempt was: + * dda_ticks_X_substeps = (int32_t)((microseconds/1000000) * f_dda * dda_substeps); */ stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time) { - // trap conditions that would prevent queueing the line - if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_EXEC) { - return (cm_hard_alarm(STAT_INTERNAL_ERROR)); - } else if (isinf(segment_time)) { return (cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE)); // never supposed to happen - } else if (isnan(segment_time)) { return (cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN)); // never supposed to happen - } else if (segment_time < EPSILON) { return (STAT_MINIMUM_TIME_MOVE); - } - // setup segment parameters - // - dda_ticks is the integer number of DDA clock ticks needed to play out the segment - // - ticks_X_substeps is the maximum depth of the DDA accumulator (as a negative number) - - st_pre.dda_period = _f_to_period(FREQUENCY_DDA); - st_pre.dda_ticks = (int32_t)(segment_time * 60 * FREQUENCY_DDA);// NB: converts minutes to seconds - st_pre.dda_ticks_X_substeps = st_pre.dda_ticks * DDA_SUBSTEPS; - - // setup motor parameters - - float correction_steps; - for (uint8_t motor=0; motor= 0) { // positive direction - st_pre.mot[motor].direction = DIRECTION_CW ^ st_cfg.mot[motor].polarity; - st_pre.mot[motor].step_sign = 1; - } else { - st_pre.mot[motor].direction = DIRECTION_CCW ^ st_cfg.mot[motor].polarity; - st_pre.mot[motor].step_sign = -1; - } - - // Detect segment time changes and setup the accumulator correction factor and flag. - // Putting this here computes the correct factor even if the motor was dormant for some - // number of previous moves. Correction is computed based on the last segment time actually used. - - if (fabs(segment_time - st_pre.mot[motor].prev_segment_time) > 0.0000001) { // highly tuned FP != compare - if (fp_NOT_ZERO(st_pre.mot[motor].prev_segment_time)) { // special case to skip first move - st_pre.mot[motor].accumulator_correction_flag = true; - st_pre.mot[motor].accumulator_correction = segment_time / st_pre.mot[motor].prev_segment_time; - } - st_pre.mot[motor].prev_segment_time = segment_time; - } + // trap conditions that would prevent queueing the line + if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_EXEC) { + return cm_hard_alarm(STAT_INTERNAL_ERROR); + } else if (isinf(segment_time)) { return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); // never supposed to happen + } else if (isnan(segment_time)) { return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN); // never supposed to happen + } else if (segment_time < EPSILON) { return STAT_MINIMUM_TIME_MOVE; + } + // setup segment parameters + // - dda_ticks is the integer number of DDA clock ticks needed to play out the segment + // - ticks_X_substeps is the maximum depth of the DDA accumulator (as a negative number) + + st_pre.dda_period = _f_to_period(FREQUENCY_DDA); + st_pre.dda_ticks = (int32_t)(segment_time * 60 * FREQUENCY_DDA);// NB: converts minutes to seconds + st_pre.dda_ticks_X_substeps = st_pre.dda_ticks * DDA_SUBSTEPS; + + // setup motor parameters + + float correction_steps; + for (uint8_t motor=0; motor= 0) { // positive direction + st_pre.mot[motor].direction = DIRECTION_CW ^ st_cfg.mot[motor].polarity; + st_pre.mot[motor].step_sign = 1; + } else { + st_pre.mot[motor].direction = DIRECTION_CCW ^ st_cfg.mot[motor].polarity; + st_pre.mot[motor].step_sign = -1; + } + + // Detect segment time changes and setup the accumulator correction factor and flag. + // Putting this here computes the correct factor even if the motor was dormant for some + // number of previous moves. Correction is computed based on the last segment time actually used. + + if (fabs(segment_time - st_pre.mot[motor].prev_segment_time) > 0.0000001) { // highly tuned FP != compare + if (fp_NOT_ZERO(st_pre.mot[motor].prev_segment_time)) { // special case to skip first move + st_pre.mot[motor].accumulator_correction_flag = true; + st_pre.mot[motor].accumulator_correction = segment_time / st_pre.mot[motor].prev_segment_time; + } + st_pre.mot[motor].prev_segment_time = segment_time; + } #ifdef __STEP_CORRECTION - // 'Nudge' correction strategy. Inject a single, scaled correction value then hold off - - if ((--st_pre.mot[motor].correction_holdoff < 0) && - (fabs(following_error[motor]) > STEP_CORRECTION_THRESHOLD)) { - - st_pre.mot[motor].correction_holdoff = STEP_CORRECTION_HOLDOFF; - correction_steps = following_error[motor] * STEP_CORRECTION_FACTOR; - - if (correction_steps > 0) { - correction_steps = min3(correction_steps, fabs(travel_steps[motor]), STEP_CORRECTION_MAX); - } else { - correction_steps = max3(correction_steps, -fabs(travel_steps[motor]), -STEP_CORRECTION_MAX); - } - st_pre.mot[motor].corrected_steps += correction_steps; - travel_steps[motor] -= correction_steps; - } + // 'Nudge' correction strategy. Inject a single, scaled correction value then hold off + + if ((--st_pre.mot[motor].correction_holdoff < 0) && + (fabs(following_error[motor]) > STEP_CORRECTION_THRESHOLD)) { + + st_pre.mot[motor].correction_holdoff = STEP_CORRECTION_HOLDOFF; + correction_steps = following_error[motor] * STEP_CORRECTION_FACTOR; + + if (correction_steps > 0) { + correction_steps = min3(correction_steps, fabs(travel_steps[motor]), STEP_CORRECTION_MAX); + } else { + correction_steps = max3(correction_steps, -fabs(travel_steps[motor]), -STEP_CORRECTION_MAX); + } + st_pre.mot[motor].corrected_steps += correction_steps; + travel_steps[motor] -= correction_steps; + } #endif - // Compute substeb increment. The accumulator must be *exactly* the incoming - // fractional steps times the substep multiplier or positional drift will occur. - // Rounding is performed to eliminate a negative bias in the uint32 conversion - // that results in long-term negative drift. (fabs/round order doesn't matter) + // Compute substeb increment. The accumulator must be *exactly* the incoming + // fractional steps times the substep multiplier or positional drift will occur. + // Rounding is performed to eliminate a negative bias in the uint32 conversion + // that results in long-term negative drift. (fabs/round order doesn't matter) - st_pre.mot[motor].substep_increment = round(fabs(travel_steps[motor] * DDA_SUBSTEPS)); - } - st_pre.move_type = MOVE_TYPE_ALINE; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready - return (STAT_OK); + st_pre.mot[motor].substep_increment = round(fabs(travel_steps[motor] * DDA_SUBSTEPS)); + } + st_pre.move_type = MOVE_TYPE_ALINE; + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready + return STAT_OK; } /* @@ -740,8 +740,8 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment void st_prep_null() { - st_pre.move_type = MOVE_TYPE_NULL; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // signal that prep buffer is empty + st_pre.move_type = MOVE_TYPE_0; + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // signal that prep buffer is empty } /* @@ -750,45 +750,45 @@ void st_prep_null() void st_prep_command(void *bf) { - st_pre.move_type = MOVE_TYPE_COMMAND; - st_pre.bf = (mpBuf_t *)bf; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready + st_pre.move_type = MOVE_TYPE_COMMAND; + st_pre.bf = (mpBuf_t *)bf; + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready } /* - * st_prep_dwell() - Add a dwell to the move buffer + * st_prep_dwell() - 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_DWELL); - st_pre.dda_ticks = (uint32_t)((microseconds/1000000) * FREQUENCY_DWELL); - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready + st_pre.move_type = MOVE_TYPE_DWELL; + st_pre.dda_period = _f_to_period(FREQUENCY_DWELL); + st_pre.dda_ticks = (uint32_t)((microseconds/1000000) * FREQUENCY_DWELL); + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready } /* * _set_hw_microsteps() - set microsteps in hardware * - * For now the microsteps is the same as the microsteps (1,2,4,8) - * This may change if microstep morphing is implemented. + * For now the microsteps is the same as the microsteps (1,2,4,8) + * This may change if microstep morphing is implemented. */ static void _set_hw_microsteps(const uint8_t motor, const uint8_t microsteps) { if (microsteps == 8) { - hw.st_port[motor]->OUTSET = MICROSTEP_BIT_0_bm; - hw.st_port[motor]->OUTSET = MICROSTEP_BIT_1_bm; - } else if (microsteps == 4) { - hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_0_bm; - hw.st_port[motor]->OUTSET = MICROSTEP_BIT_1_bm; - } else if (microsteps == 2) { - hw.st_port[motor]->OUTSET = MICROSTEP_BIT_0_bm; - hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_1_bm; - } else if (microsteps == 1) { - hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_0_bm; - hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_1_bm; - } + hw.st_port[motor]->OUTSET = MICROSTEP_BIT_0_bm; + hw.st_port[motor]->OUTSET = MICROSTEP_BIT_1_bm; + } else if (microsteps == 4) { + hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_0_bm; + hw.st_port[motor]->OUTSET = MICROSTEP_BIT_1_bm; + } else if (microsteps == 2) { + hw.st_port[motor]->OUTSET = MICROSTEP_BIT_0_bm; + hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_1_bm; + } else if (microsteps == 1) { + hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_0_bm; + hw.st_port[motor]->OUTCLR = MICROSTEP_BIT_1_bm; + } } @@ -803,7 +803,7 @@ static void _set_hw_microsteps(const uint8_t motor, const uint8_t microsteps) static int8_t _get_motor(const nvObj_t *nv) { - return ((nv->group[0] ? nv->group[0] : nv->token[0]) - 0x31); + return (nv->group[0] ? nv->group[0] : nv->token[0] - 0x31); } /* @@ -813,10 +813,10 @@ static int8_t _get_motor(const nvObj_t *nv) static void _set_motor_steps_per_unit(nvObj_t *nv) { - uint8_t m = _get_motor(nv); -// st_cfg.mot[m].units_per_step = (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle) / (360 * st_cfg.mot[m].microsteps); // unused + uint8_t m = _get_motor(nv); +// st_cfg.mot[m].units_per_step = (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle) / (360 * st_cfg.mot[m].microsteps); // unused st_cfg.mot[m].steps_per_unit = (360 * st_cfg.mot[m].microsteps) / (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle); - st_reset(); + st_reset(); } /* PER-MOTOR FUNCTIONS @@ -827,61 +827,61 @@ static void _set_motor_steps_per_unit(nvObj_t *nv) * st_set_pl() - set motor power level */ -stat_t st_set_sa(nvObj_t *nv) // motor step angle +stat_t st_set_sa(nvObj_t *nv) // motor step angle { - set_flt(nv); - _set_motor_steps_per_unit(nv); - return(STAT_OK); + set_flt(nv); + _set_motor_steps_per_unit(nv); + return(STAT_OK); } -stat_t st_set_tr(nvObj_t *nv) // motor travel per revolution +stat_t st_set_tr(nvObj_t *nv) // motor travel per revolution { - set_flu(nv); - _set_motor_steps_per_unit(nv); - return(STAT_OK); + set_flu(nv); + _set_motor_steps_per_unit(nv); + return(STAT_OK); } -stat_t st_set_mi(nvObj_t *nv) // motor microsteps +stat_t st_set_mi(nvObj_t *nv) // motor microsteps { uint32_t mi = (uint32_t)nv->value; - if ((mi != 1) && (mi != 2) && (mi != 4) && (mi != 8)) { - nv_add_conditional_message((const char_t *)"*** WARNING *** Setting non-standard microstep value"); - } - set_int(nv); // set it anyway, even if it's unsupported. It could also be > 255 - _set_motor_steps_per_unit(nv); - _set_hw_microsteps(_get_motor(nv), (uint8_t)nv->value); - return (STAT_OK); + if ((mi != 1) && (mi != 2) && (mi != 4) && (mi != 8)) { + nv_add_conditional_message((const char_t *)"*** WARNING *** Setting non-standard microstep value"); + } + set_int(nv); // set it anyway, even if it's unsupported. It could also be > 255 + _set_motor_steps_per_unit(nv); + _set_hw_microsteps(_get_motor(nv), (uint8_t)nv->value); + return STAT_OK; } -stat_t st_set_pm(nvObj_t *nv) // motor power mode +stat_t st_set_pm(nvObj_t *nv) // motor power mode { - if ((uint8_t)nv->value >= MOTOR_POWER_MODE_MAX_VALUE) - return (STAT_INPUT_VALUE_RANGE_ERROR); - set_ui8(nv); - return (STAT_OK); - // NOTE: The motor power callback makes these settings take effect immediately + if ((uint8_t)nv->value >= MOTOR_POWER_MODE_MAX_VALUE) + return STAT_INPUT_VALUE_RANGE_ERROR; + set_ui8(nv); + return STAT_OK; + // NOTE: The motor power callback makes these settings take effect immediately } /* * st_set_pl() - set motor power level * - * Input value may vary from 0.000 to 1.000 The setting is scaled to allowable PWM range. - * This function sets both the scaled and dynamic power levels, and applies the - * scaled value to the vref. + * Input value may vary from 0.000 to 1.000 The setting is scaled to allowable PWM range. + * This function sets both the scaled and dynamic power levels, and applies the + * scaled value to the vref. */ -stat_t st_set_pl(nvObj_t *nv) // motor power level +stat_t st_set_pl(nvObj_t *nv) // motor power level { return(STAT_OK); } /* - * st_get_pwr() - get motor enable power state + * st_get_pwr() - get motor enable power state */ stat_t st_get_pwr(nvObj_t *nv) { - nv->value = _motor_is_enabled(_get_motor(nv)); - nv->valuetype = TYPE_INTEGER; - return (STAT_OK); + nv->value = _motor_is_enabled(_get_motor(nv)); + nv->valuetype = TYPE_INTEGER; + return STAT_OK; } /* GLOBAL FUNCTIONS (SYSTEM LEVEL) @@ -890,43 +890,43 @@ stat_t st_get_pwr(nvObj_t *nv) * st_set_md() - disable motor power * st_set_me() - enable motor power * - * Calling me or md with NULL will enable or disable all motors + * Calling me or md with 0 will enable or disable all motors * Setting a value of 0 will enable or disable all motors * Setting a value from 1 to MOTORS will enable or disable that motor only */ stat_t st_set_mt(nvObj_t *nv) { - st_cfg.motor_power_timeout = min(MOTOR_TIMEOUT_SECONDS_MAX, max(nv->value, MOTOR_TIMEOUT_SECONDS_MIN)); - return (STAT_OK); + st_cfg.motor_power_timeout = min(MOTOR_TIMEOUT_SECONDS_MAX, max(nv->value, MOTOR_TIMEOUT_SECONDS_MIN)); + return STAT_OK; } -stat_t st_set_md(nvObj_t *nv) // Make sure this function is not part of initialization --> f00 +stat_t st_set_md(nvObj_t *nv) // Make sure this function is not part of initialization --> f00 { - if (((uint8_t)nv->value == 0) || (nv->valuetype == TYPE_NULL)) { - st_deenergize_motors(); - } else { + if (((uint8_t)nv->value == 0) || (nv->valuetype == TYPE_0)) { + st_deenergize_motors(); + } else { uint8_t motor = (uint8_t)nv->value; if (motor > MOTORS) { - return (STAT_INPUT_VALUE_RANGE_ERROR); + return STAT_INPUT_VALUE_RANGE_ERROR; } _deenergize_motor(motor-1); // adjust so that motor 1 is actually 0 (etc) - } - return (STAT_OK); + } + return STAT_OK; } -stat_t st_set_me(nvObj_t *nv) // Make sure this function is not part of initialization --> f00 +stat_t st_set_me(nvObj_t *nv) // Make sure this function is not part of initialization --> f00 { - if (((uint8_t)nv->value == 0) || (nv->valuetype == TYPE_NULL)) { - st_energize_motors(); - } else { + if (((uint8_t)nv->value == 0) || (nv->valuetype == TYPE_0)) { + st_energize_motors(); + } else { uint8_t motor = (uint8_t)nv->value; if (motor > MOTORS) { - return (STAT_INPUT_VALUE_RANGE_ERROR); + return STAT_INPUT_VALUE_RANGE_ERROR; } - _energize_motor(motor-1); // adjust so that motor 1 is actually 0 (etc) - } - return (STAT_OK); + _energize_motor(motor-1); // adjust so that motor 1 is actually 0 (etc) + } + return STAT_OK; } /*********************************************************************************** @@ -936,7 +936,7 @@ stat_t st_set_me(nvObj_t *nv) // Make sure this function is not part of initiali #ifdef __TEXT_MODE -static const char msg_units0[] PROGMEM = " in"; // used by generic print functions +static const char msg_units0[] PROGMEM = " in"; // used by generic print functions static const char msg_units1[] PROGMEM = " mm"; static const char msg_units2[] PROGMEM = " deg"; static const char *const msg_units[] PROGMEM = { msg_units0, msg_units1, msg_units2 }; @@ -960,22 +960,22 @@ void st_print_md(nvObj_t *nv) { text_print_nul(nv, fmt_md);} static void _print_motor_ui8(nvObj_t *nv, const char *format) { - fprintf_P(stderr, format, nv->group, nv->token, nv->group, (uint8_t)nv->value); + fprintf_P(stderr, format, nv->group, nv->token, nv->group, (uint8_t)nv->value); } static void _print_motor_flt_units(nvObj_t *nv, const char *format, uint8_t units) { - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value, GET_TEXT_ITEM(msg_units, units)); + fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value, GET_TEXT_ITEM(msg_units, units)); } static void _print_motor_flt(nvObj_t *nv, const char *format) { - fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value); + fprintf_P(stderr, format, nv->group, nv->token, nv->group, nv->value); } static void _print_motor_pwr(nvObj_t *nv, const char *format) { - fprintf_P(stderr, format, nv->token[0], nv->value); + fprintf_P(stderr, format, nv->token[0], nv->value); } void st_print_ma(nvObj_t *nv) { _print_motor_ui8(nv, fmt_0ma);} diff --git a/src/stepper.h b/src/stepper.h index f2fd196..fe913e6 100755 --- a/src/stepper.h +++ b/src/stepper.h @@ -26,216 +26,216 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* - * Coordinated motion (line drawing) is performed using a classic Bresenham DDA. - * A number of additional steps are taken to optimize interpolation and pulse train - * timing accuracy to minimize pulse jitter and make for very smooth motion and surface - * finish. + * Coordinated motion (line drawing) is performed using a classic Bresenham DDA. + * A number of additional steps are taken to optimize interpolation and pulse train + * timing accuracy to minimize pulse jitter and make for very smooth motion and surface + * finish. * * - The DDA is not used as a 'ramp' for acceleration management. Accel is computed - * upstream in the motion planner as 3rd order (controlled jerk) equations. These - * generate accel/decel segments that are passed to the DDA for step output. + * upstream in the motion planner as 3rd order (controlled jerk) equations. These + * generate accel/decel segments that are passed to the DDA for step output. * - * - The DDA accepts and processes fractional motor steps as floating point nuymbers - * from the planner. Steps do not need to be whole numbers, and are not expected to be. - * The step values are converted to integer by multiplying by a fixed-point precision - * (DDA_SUBSTEPS, 100000). Rounding is performed to avoid a truncation bias. + * - The DDA accepts and processes fractional motor steps as floating point nuymbers + * from the planner. Steps do not need to be whole numbers, and are not expected to be. + * The step values are converted to integer by multiplying by a fixed-point precision + * (DDA_SUBSTEPS, 100000). Rounding is performed to avoid a truncation bias. * * - Constant Rate DDA clock: The DDA runs at a constant, maximum rate for every - * segment regardless of actual step rate required. This means that the DDA clock - * is not tuned to the step rate (or a multiple) of the major axis, as is typical - * for most DDAs. Running the DDA flat out might appear to be "wasteful", but it ensures - * that the best aliasing results are achieved, and is part of maintaining step accuracy - * across motion segments. - * - * The observation is that TinyG is a hard real-time system in which every clock cycle - * is knowable and can be accounted for. So if the system is capable of sustaining - * max pulse rate for the fastest move, it's capable of sustaining this rate for any - * move. So we just run it flat out and get the best pulse resolution for all moves. - * If we were running from batteries or otherwise cared about the energy budget we - * might not be so cavalier about this. - * - * At 50 KHz constant clock rate we have 20 uSec between pulse timer (DDA) interrupts. - * On the Xmega we consume <10 uSec in the interrupt - a whopping 50% of available cycles - * going into pulse generation. + * segment regardless of actual step rate required. This means that the DDA clock + * is not tuned to the step rate (or a multiple) of the major axis, as is typical + * for most DDAs. Running the DDA flat out might appear to be "wasteful", but it ensures + * that the best aliasing results are achieved, and is part of maintaining step accuracy + * across motion segments. + * + * The observation is that TinyG is a hard real-time system in which every clock cycle + * is knowable and can be accounted for. So if the system is capable of sustaining + * max pulse rate for the fastest move, it's capable of sustaining this rate for any + * move. So we just run it flat out and get the best pulse resolution for all moves. + * If we were running from batteries or otherwise cared about the energy budget we + * might not be so cavalier about this. + * + * At 50 KHz constant clock rate we have 20 uSec between pulse timer (DDA) interrupts. + * On the Xmega we consume <10 uSec in the interrupt - a whopping 50% of available cycles + * going into pulse generation. * * - Pulse timing is also helped by minimizing the time spent loading the next move - * segment. The time budget for the load is less than the time remaining before the - * next DDA clock tick. This means that the load must take < 10 uSec or the time - * between pulses will stretch out when changing segments. This does not affect - * positional accuracy but it would affect jitter and smoothness. To this end as much - * as possible about that move is pre-computed during move execution (prep cycles). - * Also, all moves are loaded from the DDA interrupt level (HI), avoiding the need - * for mutual exclusion locking or volatiles (which slow things down). + * segment. The time budget for the load is less than the time remaining before the + * next DDA clock tick. This means that the load must take < 10 uSec or the time + * between pulses will stretch out when changing segments. This does not affect + * positional accuracy but it would affect jitter and smoothness. To this end as much + * as possible about that move is pre-computed during move execution (prep cycles). + * Also, all moves are loaded from the DDA interrupt level (HI), avoiding the need + * for mutual exclusion locking or volatiles (which slow things down). */ /* **** Move generation overview and timing illustration **** * - * This ASCII art illustrates a 4 segment move to show stepper sequencing timing. + * This ASCII art illustrates a 4 segment move to show stepper sequencing timing. * * LOAD/STEP (~5000uSec) [L1][segment1][L2][segment2][L3][segment3][L4][segment4][Lb1] * PREP (100 uSec) [P1] [P2] [P3] [P4] [Pb1] * EXEC (400 uSec) [EXEC1] [EXEC2] [EXEC3] [EXEC4] [EXECb1] * PLAN (<4ms) [planmoveA][plan move B][plan move C][plan move D][plan move E] etc. * - * The move begins with the planner PLANning move A [planmoveA]. When this is done the - * computations for the first segment of move A's S curve are performed by the planner - * runtime, EXEC1. The runtime computes the number of segments and the segment-by-segment - * accelerations and decelerations for the move. Each call to EXEC generates the values - * for the next segment to be run. Once the move is running EXEC is executed as a - * callback from the step loader. - * - * When the runtime calculations are done EXEC calls the segment PREParation function [P1]. - * PREP turns the EXEC results into values needed for the loader and does some encoder work. - * The combined exec and prep take about 400 uSec. - * - * PREP takes care of heavy numerics and other cycle-intesive operations so the step loader - * L1 can run as fast as possible. The time budget for LOAD is about 10 uSec. In the diagram, - * when P1 is done segment 1 is loaded into the stepper runtime [L1] - * - * Once the segment is loaded it will pulse out steps for the duration of the segment. - * Segment timing can vary, but segments take around 5 Msec to pulse out, which is 250 DDA - * ticks at a 50 KHz step clock. - * - * Now the move is pulsing out segment 1 (at HI interrupt level). Once the L1 loader is - * finished it invokes the exec function for the next segment (at LO interrupt level). - * [EXEC2] and [P2] compute and prepare the segment 2 for the loader so it can be loaded - * as soon as segment 1 is complete [L2]. When move A is done EXEC pulls the next move - * (moveB) from the planner queue, The process repeats until there are no more segments or moves. - * - * While all this is happening subsequent moves (B, C, and D) are being planned in background. - * As long as a move takes less than the segment times (5ms x N) the timing budget is satisfied, - * - * A few things worth noting: - * - This scheme uses 2 interrupt levels and background, for 3 levels of execution: - * - STEP pulsing and LOADs occur at HI interrupt level - * - EXEC and PREP occur at LO interrupt level (leaving MED int level for serial IO) - * - move PLANning occurs in background and is managed by the controller - * - * - Because of the way the timing is laid out there is no contention for resources between - * the STEP, LOAD, EXEC, and PREP phases. PLANing is similarly isolated. Very few volatiles - * or mutexes are needed, which makes the code simpler and faster. With the exception of - * the actual values used in step generation (which runs continuously) you can count on - * LOAD, EXEC, PREP and PLAN not stepping on each other's variables. + * The move begins with the planner PLANning move A [planmoveA]. When this is done the + * computations for the first segment of move A's S curve are performed by the planner + * runtime, EXEC1. The runtime computes the number of segments and the segment-by-segment + * accelerations and decelerations for the move. Each call to EXEC generates the values + * for the next segment to be run. Once the move is running EXEC is executed as a + * callback from the step loader. + * + * When the runtime calculations are done EXEC calls the segment PREParation function [P1]. + * PREP turns the EXEC results into values needed for the loader and does some encoder work. + * The combined exec and prep take about 400 uSec. + * + * PREP takes care of heavy numerics and other cycle-intesive operations so the step loader + * L1 can run as fast as possible. The time budget for LOAD is about 10 uSec. In the diagram, + * when P1 is done segment 1 is loaded into the stepper runtime [L1] + * + * Once the segment is loaded it will pulse out steps for the duration of the segment. + * Segment timing can vary, but segments take around 5 Msec to pulse out, which is 250 DDA + * ticks at a 50 KHz step clock. + * + * Now the move is pulsing out segment 1 (at HI interrupt level). Once the L1 loader is + * finished it invokes the exec function for the next segment (at LO interrupt level). + * [EXEC2] and [P2] compute and prepare the segment 2 for the loader so it can be loaded + * as soon as segment 1 is complete [L2]. When move A is done EXEC pulls the next move + * (moveB) from the planner queue, The process repeats until there are no more segments or moves. + * + * While all this is happening subsequent moves (B, C, and D) are being planned in background. + * As long as a move takes less than the segment times (5ms x N) the timing budget is satisfied, + * + * A few things worth noting: + * - This scheme uses 2 interrupt levels and background, for 3 levels of execution: + * - STEP pulsing and LOADs occur at HI interrupt level + * - EXEC and PREP occur at LO interrupt level (leaving MED int level for serial IO) + * - move PLANning occurs in background and is managed by the controller + * + * - Because of the way the timing is laid out there is no contention for resources between + * the STEP, LOAD, EXEC, and PREP phases. PLANing is similarly isolated. Very few volatiles + * or mutexes are needed, which makes the code simpler and faster. With the exception of + * the actual values used in step generation (which runs continuously) you can count on + * LOAD, EXEC, PREP and PLAN not stepping on each other's variables. */ /**** Line planning and execution (in more detail) **** * - * Move planning, execution and pulse generation takes place at 3 levels: + * Move planning, execution and pulse generation takes place at 3 levels: * - * Move planning occurs in the main-loop. The canonical machine calls the planner to - * generate lines, arcs, dwells, synchronous stop/starts, and any other cvommand that - * needs to be syncronized wsith motion. The planner module generates blocks (bf's) - * that hold parameters for lines and the other move types. The blocks are backplanned - * to join lines and to take dwells and stops into account. ("plan" stage). + * Move planning occurs in the main-loop. The canonical machine calls the planner to + * generate lines, arcs, dwells, synchronous stop/starts, and any other cvommand that + * needs to be syncronized wsith motion. The planner module generates blocks (bf's) + * that hold parameters for lines and the other move types. The blocks are backplanned + * to join lines and to take dwells and stops into account. ("plan" stage). * - * Arc movement is planned above the line planner. The arc planner generates short - * lines that are passed to the line planner. + * Arc movement is planned above the line planner. The arc planner generates short + * lines that are passed to the line planner. * - * Once lines are planned the must be broken up into "segments" of about 5 milliseconds - * to be run. These segments are how S curves are generated. This is the job of the move - * runtime (aka. exec or mr). + * Once lines are planned the must be broken up into "segments" of about 5 milliseconds + * to be run. These segments are how S curves are generated. This is the job of the move + * runtime (aka. exec or mr). * - * Move execution and load prep takes place at the LOW interrupt level. Move execution - * generates the next acceleration, cruise, or deceleration segment for planned lines, - * or just transfers parameters needed for dwells and stops. This layer also prepares - * segments for loading by pre-calculating the values needed by the DDA and converting - * the segment into parameters that can be directly loaded into the steppers ("exec" - * and "prep" stages). + * Move execution and load prep takes place at the LOW interrupt level. Move execution + * generates the next acceleration, cruise, or deceleration segment for planned lines, + * or just transfers parameters needed for dwells and stops. This layer also prepares + * segments for loading by pre-calculating the values needed by the DDA and converting + * the segment into parameters that can be directly loaded into the steppers ("exec" + * and "prep" stages). * - * Pulse train generation takes place at the HI interrupt level. The stepper DDA fires - * timer interrupts that generate the stepper pulses. This level also transfers new - * stepper parameters once each pulse train ("segment") is complete ("load" and "run" stages). + * Pulse train generation takes place at the HI interrupt level. The stepper DDA fires + * timer interrupts that generate the stepper pulses. This level also transfers new + * stepper parameters once each pulse train ("segment") is complete ("load" and "run" stages). */ -/* What happens when the pulse generator is done with the current pulse train (segment) - * is a multi-stage "pull" queue that looks like this: - * - * As long as the steppers are running the sequence of events is: - * - * - The stepper interrupt (HI) runs the DDA to generate a pulse train for the - * current move. This runs for the length of the pulse train currently executing - * - the "segment", usually 5ms worth of pulses - * - * - When the current segment is finished the stepper interrupt LOADs the next segment - * from the prep buffer, reloads the timers, and starts the next segment. At the end - * of the load the stepper interrupt routine requests an "exec" of the next move in - * order to prepare for the next load operation. It does this by calling the exec - * using a software interrupt (actually a timer, since that's all we've got). - * - * - As a result of the above, the EXEC handler fires at the LO interrupt level. It - * computes the next accel/decel or cruise (body) segment for the current move - * (i.e. the move in the planner's runtime buffer) by calling back to the exec - * routine in planner.c. If there are no more segments to run for the move the - * exec first gets the next buffer in the planning queue and begins execution. - * - * In some cases the mext "move" is not actually a move, but a dewll, stop, IO - * operation (e.g. M5). In this case it executes the requested operation, and may - * attempt to get the next buffer from the planner when its done. - * - * - Once the segment has been computed the exec handler finshes up by running the - * PREP routine in stepper.c. This computes the DDA values and gets the segment - * into the prep buffer - and ready for the next LOAD operation. - * - * - The main loop runs in background to receive gcode blocks, parse them, and send - * them to the planner in order to keep the planner queue full so that when the - * planner's runtime buffer completes the next move (a gcode block or perhaps an - * arc segment) is ready to run. - * - * If the steppers are not running the above is similar, except that the exec is - * invoked from the main loop by the software interrupt, and the stepper load is - * invoked from the exec by another software interrupt. +/* What happens when the pulse generator is done with the current pulse train (segment) + * is a multi-stage "pull" queue that looks like this: + * + * As long as the steppers are running the sequence of events is: + * + * - The stepper interrupt (HI) runs the DDA to generate a pulse train for the + * current move. This runs for the length of the pulse train currently executing + * - the "segment", usually 5ms worth of pulses + * + * - When the current segment is finished the stepper interrupt LOADs the next segment + * from the prep buffer, reloads the timers, and starts the next segment. At the end + * of the load the stepper interrupt routine requests an "exec" of the next move in + * order to prepare for the next load operation. It does this by calling the exec + * using a software interrupt (actually a timer, since that's all we've got). + * + * - As a result of the above, the EXEC handler fires at the LO interrupt level. It + * computes the next accel/decel or cruise (body) segment for the current move + * (i.e. the move in the planner's runtime buffer) by calling back to the exec + * routine in planner.c. If there are no more segments to run for the move the + * exec first gets the next buffer in the planning queue and begins execution. + * + * In some cases the mext "move" is not actually a move, but a dewll, stop, IO + * operation (e.g. M5). In this case it executes the requested operation, and may + * attempt to get the next buffer from the planner when its done. + * + * - Once the segment has been computed the exec handler finshes up by running the + * PREP routine in stepper.c. This computes the DDA values and gets the segment + * into the prep buffer - and ready for the next LOAD operation. + * + * - The main loop runs in background to receive gcode blocks, parse them, and send + * them to the planner in order to keep the planner queue full so that when the + * planner's runtime buffer completes the next move (a gcode block or perhaps an + * arc segment) is ready to run. + * + * If the steppers are not running the above is similar, except that the exec is + * invoked from the main loop by the software interrupt, and the stepper load is + * invoked from the exec by another software interrupt. */ -/* Control flow can be a bit confusing. This is a typical sequence for planning - * executing, and running an acceleration planned line: +/* Control flow can be a bit confusing. This is a typical sequence for planning + * executing, and running an acceleration planned line: * - * 1 planner.mp_aline() is called, which populates a planning buffer (bf) - * and back-plans any pre-existing buffers. + * 1 planner.mp_aline() is called, which populates a planning buffer (bf) + * and back-plans any pre-existing buffers. * - * 2 When a new buffer is added _mp_queue_write_buffer() tries to invoke - * execution of the move by calling stepper.st_request_exec_move(). + * 2 When a new buffer is added _mp_queue_write_buffer() tries to invoke + * execution of the move by calling stepper.st_request_exec_move(). * - * 3a If the steppers are running this request is ignored. - * 3b If the steppers are not running this will set a timer to cause an - * EXEC "software interrupt" that will ultimately call st_exec_move(). + * 3a If the steppers are running this request is ignored. + * 3b If the steppers are not running this will set a timer to cause an + * EXEC "software interrupt" that will ultimately call st_exec_move(). * * 4 At this point a call to _exec_move() is made, either by the - * software interrupt from 3b, or once the steppers finish running - * the current segment and have loaded the next segment. In either - * case the call is initated via the EXEC software interrupt which - * causes _exec_move() to run at the MEDium interupt level. + * software interrupt from 3b, or once the steppers finish running + * the current segment and have loaded the next segment. In either + * case the call is initated via the EXEC software interrupt which + * causes _exec_move() to run at the MEDium interupt level. * - * 5 _exec_move() calls back to planner.mp_exec_move() which generates - * the next segment using the mr singleton. + * 5 _exec_move() calls back to planner.mp_exec_move() which generates + * the next segment using the mr singleton. * - * 6 When this operation is complete mp_exec_move() calls the appropriate - * PREP routine in stepper.c to derive the stepper parameters that will - * be needed to run the move - in this example st_prep_line(). + * 6 When this operation is complete mp_exec_move() calls the appropriate + * PREP routine in stepper.c to derive the stepper parameters that will + * be needed to run the move - in this example st_prep_line(). * - * 7 st_prep_line() generates the timer and DDA values and stages these into - * the prep structure (sp) - ready for loading into the stepper runtime struct + * 7 st_prep_line() generates the timer and DDA values and stages these into + * the prep structure (sp) - ready for loading into the stepper runtime struct * - * 8 stepper.st_prep_line() returns back to planner.mp_exec_move(), which - * frees the planning buffer (bf) back to the planner buffer pool if the - * move is complete. This is done by calling _mp_request_finalize_run_buffer() + * 8 stepper.st_prep_line() returns back to planner.mp_exec_move(), which + * frees the planning buffer (bf) back to the planner buffer pool if the + * move is complete. This is done by calling _mp_request_finalize_run_buffer() * - * 9 At this point the MED interrupt is complete, but the planning buffer has - * not actually been returned to the pool yet. The buffer will be returned - * by the main-loop prior to testing for an available write buffer in order - * to receive the next Gcode block. This handoff prevents possible data - * conflicts between the interrupt and main loop. + * 9 At this point the MED interrupt is complete, but the planning buffer has + * not actually been returned to the pool yet. The buffer will be returned + * by the main-loop prior to testing for an available write buffer in order + * to receive the next Gcode block. This handoff prevents possible data + * conflicts between the interrupt and main loop. * - * 10 The final step in the sequence is _load_move() requesting the next - * segment to be executed and prepared by calling st_request_exec() - * - control goes back to step 4. + * 10 The final step in the sequence is _load_move() requesting the next + * segment to be executed and prepared by calling st_request_exec() + * - control goes back to step 4. * - * Note: For this to work you have to be really careful about what structures - * are modified at what level, and use volatiles where necessary. + * Note: For this to work you have to be really careful about what structures + * are modified at what level, and use volatiles where necessary. */ /* Partial steps and phase angle compensation * - * The DDA accepts partial steps as input. Fractional steps are managed by the - * sub-step value as explained elsewhere. The fraction initially loaded into - * the DDA and the remainder left at the end of a move (the "residual") can - * be thought of as a phase angle value for the DDA accumulation. Each 360 - * degrees of phase angle results in a step being generated. + * The DDA accepts partial steps as input. Fractional steps are managed by the + * sub-step value as explained elsewhere. The fraction initially loaded into + * the DDA and the remainder left at the end of a move (the "residual") can + * be thought of as a phase angle value for the DDA accumulation. Each 360 + * degrees of phase angle results in a step being generated. */ #ifndef STEPPER_H_ONCE #define STEPPER_H_ONCE @@ -246,185 +246,185 @@ //See hardware.h for platform specific stepper definitions enum prepBufferState { - PREP_BUFFER_OWNED_BY_LOADER = 0, // staging buffer is ready for load - PREP_BUFFER_OWNED_BY_EXEC // staging buffer is being loaded + PREP_BUFFER_OWNED_BY_LOADER = 0, // staging buffer is ready for load + PREP_BUFFER_OWNED_BY_EXEC // staging buffer is being loaded }; // Currently there is no distinction between IDLE and OFF (DEENERGIZED) // In the future IDLE will be powered at a low, torque-maintaining current -enum motorPowerState { // used w/start and stop flags to sequence motor power - MOTOR_OFF = 0, // motor is stopped and deenergized - MOTOR_IDLE, // motor is stopped and may be partially energized for torque maintenance - MOTOR_RUNNING, // motor is running (and fully energized) - MOTOR_POWER_TIMEOUT_START, // transitional state to start power-down timeout - MOTOR_POWER_TIMEOUT_COUNTDOWN // count down the time to de-energizing motors +enum motorPowerState { // used w/start and stop flags to sequence motor power + MOTOR_OFF = 0, // motor is stopped and deenergized + MOTOR_IDLE, // motor is stopped and may be partially energized for torque maintenance + MOTOR_RUNNING, // motor is running (and fully energized) + MOTOR_POWER_TIMEOUT_START, // transitional state to start power-down timeout + MOTOR_POWER_TIMEOUT_COUNTDOWN // count down the time to de-energizing motors }; enum cmMotorPowerMode { - MOTOR_DISABLED = 0, // motor enable is deactivated - MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON - MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, de-powered out of cycle - MOTOR_POWERED_ONLY_WHEN_MOVING, // motor only powered while moving - idles shortly after it's stopped - even in cycle -// MOTOR_POWER_REDUCED_WHEN_IDLE, // enable Vref current reduction for idle (FUTURE) -// MOTOR_ADAPTIVE_POWER // adjust motor current with velocity (FUTURE) - MOTOR_POWER_MODE_MAX_VALUE // for input range checking + MOTOR_DISABLED = 0, // motor enable is deactivated + MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON + MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, de-powered out of cycle + MOTOR_POWERED_ONLY_WHEN_MOVING, // motor only powered while moving - idles shortly after it's stopped - even in cycle +// MOTOR_POWER_REDUCED_WHEN_IDLE, // enable Vref current reduction for idle (FUTURE) +// MOTOR_ADAPTIVE_POWER // adjust motor current with velocity (FUTURE) + MOTOR_POWER_MODE_MAX_VALUE // for input range checking }; // Min/Max timeouts allowed for motor disable. Allow for inertial stop; must be non-zero -#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 // seconds !!! SHOULD NEVER BE ZERO !!! -#define MOTOR_TIMEOUT_SECONDS_MAX (float)4294967 // (4294967295/1000) -- for conversion to uint32_t -#define MOTOR_TIMEOUT_SECONDS (float)0.1 // seconds in DISABLE_AXIS_WHEN_IDLE mode -#define MOTOR_TIMEOUT_WHEN_MOVING (float)0.25 // timeout for a motor in _ONLY_WHEN_MOVING mode +#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 // seconds !!! SHOULD NEVER BE ZERO !!! +#define MOTOR_TIMEOUT_SECONDS_MAX (float)4294967 // (4294967295/1000) -- for conversion to uint32_t +#define MOTOR_TIMEOUT_SECONDS (float)0.1 // seconds in DISABLE_AXIS_WHEN_IDLE mode +#define MOTOR_TIMEOUT_WHEN_MOVING (float)0.25 // timeout for a motor in _ONLY_WHEN_MOVING mode /* DDA substepping - * DDA Substepping is a fixed.point scheme to increase the resolution of the DDA pulse generation - * while still using integer math (as opposed to floating point). Improving the accuracy of the DDA - * results in more precise pulse timing and therefore less pulse jitter and smoother motor operation. + * DDA Substepping is a fixed.point scheme to increase the resolution of the DDA pulse generation + * while still using integer math (as opposed to floating point). Improving the accuracy of the DDA + * results in more precise pulse timing and therefore less pulse jitter and smoother motor operation. * - * The DDA accumulator is an int32_t, so the accumulator has the number range of about 2.1 billion. - * The DDA_SUBSTEPS is used to multiply the step count for a segment to maximally use this number range. - * DDA_SUBSTEPS can be computed for a given DDA clock rate and segment time not to exceed the available - * number range. Variables are: + * The DDA accumulator is an int32_t, so the accumulator has the number range of about 2.1 billion. + * The DDA_SUBSTEPS is used to multiply the step count for a segment to maximally use this number range. + * DDA_SUBSTEPS can be computed for a given DDA clock rate and segment time not to exceed the available + * number range. Variables are: * - * MAX_LONG == 2^31, maximum signed long (depth of accumulator. NB: accumulator values are negative) - * FREQUENCY_DDA == DDA clock rate in Hz. - * NOM_SEGMENT_TIME == upper bound of segment time in minutes - * 0.90 == a safety factor used to reduce the result from theoretical maximum + * MAX_LONG == 2^31, maximum signed long (depth of accumulator. NB: accumulator values are negative) + * FREQUENCY_DDA == DDA clock rate in Hz. + * NOM_SEGMENT_TIME == upper bound of segment time in minutes + * 0.90 == a safety factor used to reduce the result from theoretical maximum * - * The number is about 8.5 million for the Xmega running a 50 KHz DDA with 5 millisecond segments + * The number is about 8.5 million for the Xmega running a 50 KHz DDA with 5 millisecond segments */ #define DDA_SUBSTEPS ((MAX_LONG * 0.90) / (FREQUENCY_DDA * (NOM_SEGMENT_TIME * 60))) /* Step correction settings - * Step correction settings determine how the encoder error is fed back to correct position errors. - * Since the following_error is running 2 segments behind the current segment you have to be careful - * not to overcompensate. The threshold determines if a correction should be applied, and the factor - * is how much. The holdoff is how many segments to wait before applying another correction. If threshold - * is too small and/or amount too large and/or holdoff is too small you may get a runaway correction - * and error will grow instead of shrink (or oscillate). + * Step correction settings determine how the encoder error is fed back to correct position errors. + * Since the following_error is running 2 segments behind the current segment you have to be careful + * not to overcompensate. The threshold determines if a correction should be applied, and the factor + * is how much. The holdoff is how many segments to wait before applying another correction. If threshold + * is too small and/or amount too large and/or holdoff is too small you may get a runaway correction + * and error will grow instead of shrink (or oscillate). */ -#define STEP_CORRECTION_THRESHOLD (float)2.00 // magnitude of forwarding error to apply correction (in steps) -#define STEP_CORRECTION_FACTOR (float)0.25 // factor to apply to step correction for a single segment -#define STEP_CORRECTION_MAX (float)0.60 // max step correction allowed in a single segment -#define STEP_CORRECTION_HOLDOFF 5 // minimum number of segments to wait between error correction -#define STEP_INITIAL_DIRECTION DIRECTION_CW +#define STEP_CORRECTION_THRESHOLD (float)2.00 // magnitude of forwarding error to apply correction (in steps) +#define STEP_CORRECTION_FACTOR (float)0.25 // factor to apply to step correction for a single segment +#define STEP_CORRECTION_MAX (float)0.60 // max step correction allowed in a single segment +#define STEP_CORRECTION_HOLDOFF 5 // minimum number of segments to wait between error correction +#define STEP_INITIAL_DIRECTION DIRECTION_CW /* * Stepper control structures * - * There are 5 main structures involved in stepper operations; + * There are 5 main structures involved in stepper operations; * - * data structure: found in: runs primarily at: - * mpBuffer planning buffers (bf) planner.c main loop - * mrRuntimeSingleton (mr) planner.c MED ISR - * stConfig (st_cfg) stepper.c write=bkgd, read=ISRs - * stPrepSingleton (st_pre) stepper.c MED ISR - * stRunSingleton (st_run) stepper.c HI ISR + * data structure: found in: runs primarily at: + * mpBuffer planning buffers (bf) planner.c main loop + * mrRuntimeSingleton (mr) planner.c MED ISR + * stConfig (st_cfg) stepper.c write=bkgd, read=ISRs + * stPrepSingleton (st_pre) stepper.c MED ISR + * stRunSingleton (st_run) stepper.c HI ISR * - * Care has been taken to isolate actions on these structures to the execution level - * in which they run and to use the minimum number of volatiles in these structures. - * This allows the compiler to optimize the stepper inner-loops better. + * Care has been taken to isolate actions on these structures to the execution level + * in which they run and to use the minimum number of volatiles in these structures. + * This allows the compiler to optimize the stepper inner-loops better. */ // Motor config structure -typedef struct cfgMotor { // per-motor configs - // public - uint8_t motor_map; // map motor to axis - uint32_t microsteps; // microsteps to apply for each axis (ex: 8) - uint8_t polarity; // 0=normal polarity, 1=reverse motor direction - uint8_t power_mode; // See cmMotorPowerMode for enum - float power_level; // set 0.000 to 1.000 for PMW vref setting - float step_angle; // degrees per whole step (ex: 1.8) - float travel_rev; // mm or deg of travel per motor revolution - float steps_per_unit; // microsteps per mm (or degree) of travel - float units_per_step; // mm or degrees of travel per microstep - - // private - float power_level_scaled; // scaled to internal range - must be between 0 and 1 +typedef struct cfgMotor { // per-motor configs + // public + uint8_t motor_map; // map motor to axis + uint32_t microsteps; // microsteps to apply for each axis (ex: 8) + uint8_t polarity; // 0=normal polarity, 1=reverse motor direction + uint8_t power_mode; // See cmMotorPowerMode for enum + float power_level; // set 0.000 to 1.000 for PMW vref setting + float step_angle; // degrees per whole step (ex: 1.8) + float travel_rev; // mm or deg of travel per motor revolution + float steps_per_unit; // microsteps per mm (or degree) of travel + float units_per_step; // mm or degrees of travel per microstep + + // private + float power_level_scaled; // scaled to internal range - must be between 0 and 1 } cfgMotor_t; -typedef struct stConfig { // stepper configs - float motor_power_timeout; // seconds before setting motors to idle current (currently this is OFF) - cfgMotor_t mot[MOTORS]; // settings for motors 1-N +typedef struct stConfig { // stepper configs + float motor_power_timeout; // seconds before setting motors to idle current (currently this is OFF) + cfgMotor_t mot[MOTORS]; // settings for motors 1-N } stConfig_t; // Motor runtime structure. Used exclusively by step generation ISR (HI) -typedef struct stRunMotor { // one per controlled motor - uint32_t substep_increment; // total steps in axis times substeps factor - int32_t substep_accumulator; // DDA phase angle accumulator - uint8_t power_state; // state machine for managing motor power - uint32_t power_systick; // sys_tick for next motor power state transition +typedef struct stRunMotor { // one per controlled motor + uint32_t substep_increment; // total steps in axis times substeps factor + int32_t substep_accumulator; // DDA phase angle accumulator + uint8_t power_state; // state machine for managing motor power + uint32_t power_systick; // sys_tick for next motor power state transition } stRunMotor_t; -typedef struct stRunSingleton { // Stepper static values and axis parameters - uint16_t magic_start; // magic number to test memory integrity - uint32_t dda_ticks_downcount; // tick down-counter (unscaled) - uint32_t dda_ticks_X_substeps; // ticks multiplied by scaling factor - stRunMotor_t mot[MOTORS]; // runtime motor structures - uint16_t magic_end; +typedef struct stRunSingleton { // Stepper static values and axis parameters + uint16_t magic_start; // magic number to test memory integrity + uint32_t dda_ticks_downcount; // tick down-counter (unscaled) + uint32_t dda_ticks_X_substeps; // ticks multiplied by scaling factor + stRunMotor_t mot[MOTORS]; // runtime motor structures + uint16_t magic_end; } stRunSingleton_t; // Motor prep structure. Used by exec/prep ISR (MED) and read-only during load // Must be careful about volatiles in this one typedef struct stPrepMotor { - uint32_t substep_increment; // total steps in axis times substep factor + uint32_t substep_increment; // total steps in axis times substep factor - // direction and direction change - int8_t direction; // travel direction corrected for polarity - uint8_t prev_direction; // travel direction from previous segment run for this motor - int8_t step_sign; // set to +1 or -1 for encoders + // direction and direction change + int8_t direction; // travel direction corrected for polarity + uint8_t prev_direction; // travel direction from previous segment run for this motor + int8_t step_sign; // set to +1 or -1 for encoders - // following error correction - int32_t correction_holdoff; // count down segments between corrections - float corrected_steps; // accumulated correction steps for the cycle (for diagnostic display only) + // following error correction + int32_t correction_holdoff; // count down segments between corrections + float corrected_steps; // accumulated correction steps for the cycle (for diagnostic display only) - // accumulator phase correction - float prev_segment_time; // segment time from previous segment run for this motor - float accumulator_correction; // factor for adjusting accumulator between segments - uint8_t accumulator_correction_flag;// signals accumulator needs correction + // accumulator phase correction + float prev_segment_time; // segment time from previous segment run for this motor + float accumulator_correction; // factor for adjusting accumulator between segments + uint8_t accumulator_correction_flag;// signals accumulator needs correction } stPrepMotor_t; typedef struct stPrepSingleton { - uint16_t magic_start; // magic number to test memory integrity - volatile uint8_t buffer_state; // prep buffer state - owned by exec or loader - struct mpBuffer *bf; // static pointer to relevant buffer - uint8_t move_type; // move type - - uint16_t dda_period; // DDA or dwell clock period setting - uint32_t dda_ticks; // DDA or dwell ticks for the move - uint32_t dda_ticks_X_substeps; // DDA ticks scaled by substep factor - stPrepMotor_t mot[MOTORS]; // prep time motor structs - uint16_t magic_end; + uint16_t magic_start; // magic number to test memory integrity + volatile uint8_t buffer_state; // prep buffer state - owned by exec or loader + struct mpBuffer *bf; // static pointer to relevant buffer + uint8_t move_type; // move type + + uint16_t dda_period; // DDA or dwell clock period setting + uint32_t dda_ticks; // DDA or dwell ticks for the move + uint32_t dda_ticks_X_substeps; // DDA ticks scaled by substep factor + stPrepMotor_t mot[MOTORS]; // prep time motor structs + uint16_t magic_end; } stPrepSingleton_t; -extern stConfig_t st_cfg; // config struct is exposed. The rest are private -extern stPrepSingleton_t st_pre; // only used by config_app diagnostics +extern stConfig_t st_cfg; // config struct is exposed. The rest are private +extern stPrepSingleton_t st_pre; // only used by config_app diagnostics /**** FUNCTION PROTOTYPES ****/ -void stepper_init(void); -void stepper_init_assertions(void); -stat_t stepper_test_assertions(void); +void stepper_init(); +void stepper_init_assertions(); +stat_t stepper_test_assertions(); -uint8_t st_runtime_isbusy(void); -void st_reset(void); -void st_cycle_start(void); -void st_cycle_end(void); +uint8_t st_runtime_isbusy(); +void st_reset(); +void st_cycle_start(); +void st_cycle_end(); stat_t st_clc(nvObj_t *nv); -void st_energize_motors(void); -void st_deenergize_motors(void); +void st_energize_motors(); +void st_deenergize_motors(); void st_set_motor_power(const uint8_t motor); -stat_t st_motor_power_callback(void); +stat_t st_motor_power_callback(); -void st_request_exec_move(void); -void st_prep_null(void); -void st_prep_command(void *bf); // use a void pointer since we don't know about mpBuf_t yet) +void st_request_exec_move(); +void st_prep_null(); +void st_prep_command(void *bf); // use a void pointer since we don't know about mpBuf_t yet) void st_prep_dwell(float microseconds); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); @@ -441,32 +441,32 @@ stat_t st_set_me(nvObj_t *nv); #ifdef __TEXT_MODE - void st_print_ma(nvObj_t *nv); - void st_print_sa(nvObj_t *nv); - void st_print_tr(nvObj_t *nv); - void st_print_mi(nvObj_t *nv); - void st_print_po(nvObj_t *nv); - void st_print_pm(nvObj_t *nv); - void st_print_pl(nvObj_t *nv); - void st_print_pwr(nvObj_t *nv); - void st_print_mt(nvObj_t *nv); - void st_print_me(nvObj_t *nv); - void st_print_md(nvObj_t *nv); + void st_print_ma(nvObj_t *nv); + void st_print_sa(nvObj_t *nv); + void st_print_tr(nvObj_t *nv); + void st_print_mi(nvObj_t *nv); + void st_print_po(nvObj_t *nv); + void st_print_pm(nvObj_t *nv); + void st_print_pl(nvObj_t *nv); + void st_print_pwr(nvObj_t *nv); + void st_print_mt(nvObj_t *nv); + void st_print_me(nvObj_t *nv); + void st_print_md(nvObj_t *nv); #else - #define st_print_ma tx_print_stub - #define st_print_sa tx_print_stub - #define st_print_tr tx_print_stub - #define st_print_mi tx_print_stub - #define st_print_po tx_print_stub - #define st_print_pm tx_print_stub - #define st_print_pl tx_print_stub - #define st_print_pwr tx_print_stub - #define st_print_mt tx_print_stub - #define st_print_me tx_print_stub - #define st_print_md tx_print_stub + #define st_print_ma tx_print_stub + #define st_print_sa tx_print_stub + #define st_print_tr tx_print_stub + #define st_print_mi tx_print_stub + #define st_print_po tx_print_stub + #define st_print_pm tx_print_stub + #define st_print_pl tx_print_stub + #define st_print_pwr tx_print_stub + #define st_print_mt tx_print_stub + #define st_print_me tx_print_stub + #define st_print_md tx_print_stub #endif // __TEXT_MODE -#endif // End of include guard: STEPPER_H_ONCE +#endif // End of include guard: STEPPER_H_ONCE diff --git a/src/switch.c b/src/switch.c index b47cd06..50f88d3 100755 --- a/src/switch.c +++ b/src/switch.c @@ -24,23 +24,22 @@ * 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. */ + /* Switch Modes * - * The switches are considered to be homing switches when machine_state is - * MACHINE_HOMING. At all other times they are treated as limit switches: - * - Hitting a homing switch puts the current move into feedhold - * - Hitting a limit switch causes the machine to shut down and go into lockdown until reset + * The switches are considered to be homing switches when machine_state is + * MACHINE_HOMING. At all other times they are treated as limit switches: + * - Hitting a homing switch puts the current move into feedhold + * - Hitting a limit switch causes the machine to shut down and go into lockdown until reset * - * The normally open switch modes (NO) trigger an interrupt on the falling edge - * and lockout subsequent interrupts for the defined lockout period. This approach - * beats doing debouncing as an integration as switches fire immediately. + * The normally open switch modes (NO) trigger an interrupt on the falling edge + * and lockout subsequent interrupts for the defined lockout period. This approach + * beats doing debouncing as an integration as switches fire immediately. * - * The normally closed switch modes (NC) trigger an interrupt on the rising edge - * and lockout subsequent interrupts for the defined lockout period. Ditto on the method. + * The normally closed switch modes (NC) trigger an interrupt on the rising edge + * and lockout subsequent interrupts for the defined lockout period. Ditto on the method. */ -#include - #include "tinyg.h" #include "config.h" #include "switch.h" @@ -48,191 +47,184 @@ #include "canonical_machine.h" #include "text_parser.h" +#include + + static void _switch_isr_helper(uint8_t sw_num); /* * switch_init() - initialize homing/limit switches * - * This function assumes sys_init() and st_init() have been run previously to - * bind the ports and set bit IO directions, repsectively. See system.h for details + * This function assumes sys_init() and st_init() have been run previously to + * bind the ports and set bit IO directions, repsectively. See system.h for details */ /* Note: v7 boards have external strong pullups on GPIO2 pins (2.7K ohm). - * v6 and earlier use internal pullups only. Internal pullups are set - * regardless of board type but are extraneous for v7 boards. + * v6 and earlier use internal pullups only. Internal pullups are set + * regardless of board type but are extraneous for v7 boards. */ -#define PIN_MODE PORT_OPC_PULLUP_gc // pin mode. see iox192a3.h for details - -void switch_init(void) -{ - for (uint8_t i=0; iDIRCLR = SW_MIN_BIT_bm; // set min input - see 13.14.14 - hw.sw_port[i]->PIN6CTRL = (PIN_MODE | PORT_ISC_BOTHEDGES_gc); - hw.sw_port[i]->INT0MASK = SW_MIN_BIT_bm; // interrupt on min switch - } else { - hw.sw_port[i]->INT0MASK = 0; // disable interrupt - } - if (sw.mode[MAX_SWITCH(i)] != SW_MODE_DISABLED) { - hw.sw_port[i]->DIRCLR = SW_MAX_BIT_bm; // set max input - see 13.14.14 - hw.sw_port[i]->PIN7CTRL = (PIN_MODE | PORT_ISC_BOTHEDGES_gc); - hw.sw_port[i]->INT1MASK = SW_MAX_BIT_bm; // max on INT1 - } else { - hw.sw_port[i]->INT1MASK = 0; - } - // set interrupt levels. Interrupts must be enabled in main() - hw.sw_port[i]->INTCTRL = GPIO1_INTLVL; // see gpio.h for setting - } - reset_switches(); +#define PIN_MODE PORT_OPC_PULLUP_gc // pin mode. see iox192a3.h for details + +void switch_init() { + for (uint8_t i = 0; i < NUM_SWITCH_PAIRS; i++) { + // setup input bits and interrupts (previously set to inputs by st_init()) + if (sw.mode[MIN_SWITCH(i)] != SW_MODE_DISABLED) { + hw.sw_port[i]->DIRCLR = SW_MIN_BIT_bm; // set min input - see 13.14.14 + hw.sw_port[i]->PIN6CTRL = (PIN_MODE | PORT_ISC_BOTHEDGES_gc); + hw.sw_port[i]->INT0MASK = SW_MIN_BIT_bm; // interrupt on min switch + + } else hw.sw_port[i]->INT0MASK = 0; // disable interrupt + + if (sw.mode[MAX_SWITCH(i)] != SW_MODE_DISABLED) { + hw.sw_port[i]->DIRCLR = SW_MAX_BIT_bm; // set max input - see 13.14.14 + hw.sw_port[i]->PIN7CTRL = (PIN_MODE | PORT_ISC_BOTHEDGES_gc); + hw.sw_port[i]->INT1MASK = SW_MAX_BIT_bm; // max on INT1 + + } else hw.sw_port[i]->INT1MASK = 0; + + // set interrupt levels. Interrupts must be enabled in main() + hw.sw_port[i]->INTCTRL = GPIO1_INTLVL; // see gpio.h for setting + } + + reset_switches(); } + /* * Switch closure processing routines * - * ISRs - switch interrupt handler vectors - * _isr_helper() - common code for all switch ISRs + * ISRs - switch interrupt handler vectors + * _isr_helper() - common code for all switch ISRs * switch_rtc_callback() - called from RTC for each RTC tick. * - * These functions interact with each other to process switch closures and firing. - * Each switch has a counter which is initially set to negative SW_DEGLITCH_TICKS. - * When a switch closure is DETECTED the count increments for each RTC tick. - * When the count reaches zero the switch is tripped and action occurs. - * The counter continues to increment positive until the lockout is exceeded. + * These functions interact with each other to process switch closures and firing. + * Each switch has a counter which is initially set to negative SW_DEGLITCH_TICKS. + * When a switch closure is DETECTED the count increments for each RTC tick. + * When the count reaches zero the switch is tripped and action occurs. + * The counter continues to increment positive until the lockout is exceeded. */ +ISR(X_MIN_ISR_vect) {_switch_isr_helper(SW_MIN_X);} +ISR(Y_MIN_ISR_vect) {_switch_isr_helper(SW_MIN_Y);} +ISR(Z_MIN_ISR_vect) {_switch_isr_helper(SW_MIN_Z);} +ISR(A_MIN_ISR_vect) {_switch_isr_helper(SW_MIN_A);} +ISR(X_MAX_ISR_vect) {_switch_isr_helper(SW_MAX_X);} +ISR(Y_MAX_ISR_vect) {_switch_isr_helper(SW_MAX_Y);} +ISR(Z_MAX_ISR_vect) {_switch_isr_helper(SW_MAX_Z);} +ISR(A_MAX_ISR_vect) {_switch_isr_helper(SW_MAX_A);} -ISR(X_MIN_ISR_vect) { _switch_isr_helper(SW_MIN_X);} -ISR(Y_MIN_ISR_vect) { _switch_isr_helper(SW_MIN_Y);} -ISR(Z_MIN_ISR_vect) { _switch_isr_helper(SW_MIN_Z);} -ISR(A_MIN_ISR_vect) { _switch_isr_helper(SW_MIN_A);} -ISR(X_MAX_ISR_vect) { _switch_isr_helper(SW_MAX_X);} -ISR(Y_MAX_ISR_vect) { _switch_isr_helper(SW_MAX_Y);} -ISR(Z_MAX_ISR_vect) { _switch_isr_helper(SW_MAX_Z);} -ISR(A_MAX_ISR_vect) { _switch_isr_helper(SW_MAX_A);} - -static void _switch_isr_helper(uint8_t sw_num) -{ - if (sw.mode[sw_num] == SW_MODE_DISABLED) return; // this is never supposed to happen - if (sw.debounce[sw_num] == SW_LOCKOUT) return; // exit if switch is in lockout - sw.debounce[sw_num] = SW_DEGLITCHING; // either transitions state from IDLE or overwrites it - sw.count[sw_num] = -SW_DEGLITCH_TICKS; // reset deglitch count regardless of entry state - read_switch(sw_num); // sets the state value in the struct + +static void _switch_isr_helper(uint8_t sw_num) { + if (sw.mode[sw_num] == SW_MODE_DISABLED) return; // this is never supposed to happen + if (sw.debounce[sw_num] == SW_LOCKOUT) return; // exit if switch is in lockout + + sw.debounce[sw_num] = SW_DEGLITCHING; // either transitions state from IDLE or overwrites it + sw.count[sw_num] = -SW_DEGLITCH_TICKS; // reset deglitch count regardless of entry state + + read_switch(sw_num); // sets the state value in the struct } -void switch_rtc_callback(void) -{ - for (uint8_t i=0; i < NUM_SWITCHES; i++) { - if (sw.mode[i] == SW_MODE_DISABLED || sw.debounce[i] == SW_IDLE) - continue; - - if (++sw.count[i] == SW_LOCKOUT_TICKS) { // state is either lockout or deglitching - sw.debounce[i] = SW_IDLE; - // check if the state has changed while we were in lockout... - uint8_t old_state = sw.state[i]; - if(old_state != read_switch(i)) { - sw.debounce[i] = SW_DEGLITCHING; - sw.count[i] = -SW_DEGLITCH_TICKS; - } - continue; - } - if (sw.count[i] == 0) { // trigger point - sw.sw_num_thrown = i; // record number of thrown switch - sw.debounce[i] = SW_LOCKOUT; - - if ((cm.cycle_state == CYCLE_HOMING) || (cm.cycle_state == CYCLE_PROBE)) { // regardless of switch type - cm_request_feedhold(); - } else if (sw.mode[i] & SW_LIMIT_BIT) { // should be a limit switch, so fire it. - sw.limit_flag = true; // triggers an emergency shutdown - } - } - } + +void switch_rtc_callback() { + for (uint8_t i = 0; i < NUM_SWITCHES; i++) { + if (sw.mode[i] == SW_MODE_DISABLED || sw.debounce[i] == SW_IDLE) + continue; + + if (++sw.count[i] == SW_LOCKOUT_TICKS) { // state is either lockout or deglitching + sw.debounce[i] = SW_IDLE; + // check if the state has changed while we were in lockout... + uint8_t old_state = sw.state[i]; + if(old_state != read_switch(i)) { + sw.debounce[i] = SW_DEGLITCHING; + sw.count[i] = -SW_DEGLITCH_TICKS; + } + continue; + } + + if (sw.count[i] == 0) { // trigger point + sw.sw_num_thrown = i; // record number of thrown switch + sw.debounce[i] = SW_LOCKOUT; + + if ((cm.cycle_state == CYCLE_HOMING) || (cm.cycle_state == CYCLE_PROBE)) // regardless of switch type + cm_request_feedhold(); + + else if (sw.mode[i] & SW_LIMIT_BIT) // should be a limit switch, so fire it. + sw.limit_flag = true; // triggers an emergency shutdown + } + } } -/* - * get_switch_mode() - return switch mode setting - * get_limit_thrown() - return true if a limit was tripped - * get_switch_num() - return switch number most recently thrown - */ -uint8_t get_switch_mode(uint8_t sw_num) { return (sw.mode[sw_num]);} -uint8_t get_limit_switch_thrown(void) { return(sw.limit_flag);} -uint8_t get_switch_thrown(void) { return(sw.sw_num_thrown);} +/// return switch mode setting +uint8_t get_switch_mode(uint8_t sw_num) {return sw.mode[sw_num];} + +/// return true if a limit was tripped +uint8_t get_limit_switch_thrown() {return sw.limit_flag;} + +/// return switch number most recently thrown +uint8_t get_switch_thrown() {return sw.sw_num_thrown;} // global switch type -void set_switch_type( uint8_t switch_type ) { sw.switch_type = switch_type; } -uint8_t get_switch_type() { return sw.switch_type; } +void set_switch_type(uint8_t switch_type) {sw.switch_type = switch_type;} +uint8_t get_switch_type() {return sw.switch_type;} -/* - * reset_switches() - reset all switches and reset limit flag - */ -void reset_switches() -{ - for (uint8_t i=0; i < NUM_SWITCHES; i++) { - sw.debounce[i] = SW_IDLE; - read_switch(i); - } - sw.limit_flag = false; +/// reset all switches and reset limit flag +void reset_switches() { + for (uint8_t i = 0; i < NUM_SWITCHES; i++) { + sw.debounce[i] = SW_IDLE; + read_switch(i); + } + + sw.limit_flag = false; } -/* - * read_switch() - 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); - - uint8_t read = 0; - switch (sw_num) { - case SW_MIN_X: { read = hw.sw_port[AXIS_X]->IN & SW_MIN_BIT_bm; break;} - case SW_MAX_X: { read = hw.sw_port[AXIS_X]->IN & SW_MAX_BIT_bm; break;} - case SW_MIN_Y: { read = hw.sw_port[AXIS_Y]->IN & SW_MIN_BIT_bm; break;} - case SW_MAX_Y: { read = hw.sw_port[AXIS_Y]->IN & SW_MAX_BIT_bm; break;} - case SW_MIN_Z: { read = hw.sw_port[AXIS_Z]->IN & SW_MIN_BIT_bm; break;} - case SW_MAX_Z: { read = hw.sw_port[AXIS_Z]->IN & SW_MAX_BIT_bm; break;} - case SW_MIN_A: { read = hw.sw_port[AXIS_A]->IN & SW_MIN_BIT_bm; break;} - case SW_MAX_A: { read = hw.sw_port[AXIS_A]->IN & SW_MAX_BIT_bm; break;} - } - if (sw.switch_type == SW_TYPE_NORMALLY_OPEN) { - sw.state[sw_num] = ((read == 0) ? SW_CLOSED : SW_OPEN);// confusing. An NO switch drives the pin LO when thrown - return (sw.state[sw_num]); - } else { - sw.state[sw_num] = ((read != 0) ? SW_CLOSED : SW_OPEN); - return (sw.state[sw_num]); - } + +/// 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; + + uint8_t read = 0; + switch (sw_num) { + case SW_MIN_X: read = hw.sw_port[AXIS_X]->IN & SW_MIN_BIT_bm; break; + case SW_MAX_X: read = hw.sw_port[AXIS_X]->IN & SW_MAX_BIT_bm; break; + case SW_MIN_Y: read = hw.sw_port[AXIS_Y]->IN & SW_MIN_BIT_bm; break; + case SW_MAX_Y: read = hw.sw_port[AXIS_Y]->IN & SW_MAX_BIT_bm; break; + case SW_MIN_Z: read = hw.sw_port[AXIS_Z]->IN & SW_MIN_BIT_bm; break; + case SW_MAX_Z: read = hw.sw_port[AXIS_Z]->IN & SW_MAX_BIT_bm; break; + case SW_MIN_A: read = hw.sw_port[AXIS_A]->IN & SW_MIN_BIT_bm; break; + case SW_MAX_A: read = hw.sw_port[AXIS_A]->IN & SW_MAX_BIT_bm; break; + } + + if (sw.switch_type == SW_TYPE_NORMALLY_OPEN) + sw.state[sw_num] = (read == 0) ? SW_CLOSED : SW_OPEN; // confusing. An NO switch drives the pin LO when thrown + else sw.state[sw_num] = (read != 0) ? SW_CLOSED : SW_OPEN; + + return sw.state[sw_num]; } -/*********************************************************************************** - * CONFIGURATION AND INTERFACE FUNCTIONS - * Functions to get and set variables from the cfgArray table - * These functions are not part of the NIST defined functions - ***********************************************************************************/ +stat_t sw_set_st(nvObj_t *nv) { // switch type (global) + set_01(nv); + switch_init(); -stat_t sw_set_st(nvObj_t *nv) // switch type (global) -{ - set_01(nv); - switch_init(); - return (STAT_OK); + return STAT_OK; } -stat_t sw_set_sw(nvObj_t *nv) // switch setting -{ - if (nv->value > SW_MODE_MAX_VALUE) - return (STAT_INPUT_VALUE_RANGE_ERROR); - set_ui8(nv); - switch_init(); - return (STAT_OK); + +stat_t sw_set_sw(nvObj_t *nv) { // switch setting + if (nv->value > SW_MODE_MAX_VALUE) + return STAT_INPUT_VALUE_RANGE_ERROR; + + set_ui8(nv); + switch_init(); + + return STAT_OK; } -/*********************************************************************************** - * TEXT MODE SUPPORT - * Functions to print variables from the cfgArray table - ***********************************************************************************/ #ifdef __TEXT_MODE static const char fmt_st[] PROGMEM = "[st] switch type%18d [0=NO,1=NC]\n"; -void sw_print_st(nvObj_t *nv) { text_print_ui8(nv, fmt_st);} +void sw_print_st(nvObj_t *nv) {text_print_ui8(nv, fmt_st);} #endif diff --git a/src/switch.h b/src/switch.h index 403d91e..ba8cd55 100755 --- a/src/switch.h +++ b/src/switch.h @@ -26,104 +26,79 @@ */ /* Switch processing functions under Motate * - * Switch processing turns pin transitions into reliable switch states. - * There are 2 main operations: + * Switch processing turns pin transitions into reliable switch states. + * There are 2 main operations: * - * - read pin get raw data from a pin - * - read switch return processed switch closures + * - read pin get raw data from a pin + * - read switch return processed switch closures * - * Read pin may be a polled operation or an interrupt on pin change. If interrupts - * are used they must be provided for both leading and trailing edge transitions. + * Read pin may be a polled operation or an interrupt on pin change. If interrupts + * are used they must be provided for both leading and trailing edge transitions. * - * Read switch contains the results of read pin and manages edges and debouncing. + * Read switch contains the results of read pin and manages edges and debouncing. */ #ifndef SWITCH_H_ONCE #define SWITCH_H_ONCE -/* - * Common variables and settings - */ - // timer for debouncing switches -#define SW_LOCKOUT_TICKS 25 // 25=250ms. RTC ticks are ~10ms each -#define SW_DEGLITCH_TICKS 3 // 3=30ms +// timer for debouncing switches +#define SW_LOCKOUT_TICKS 25 // 25=250ms. RTC ticks are ~10ms each +#define SW_DEGLITCH_TICKS 3 // 3=30ms // switch modes #define SW_HOMING_BIT 0x01 #define SW_LIMIT_BIT 0x02 -#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 -#define SW_MODE_HOMING_LIMIT (SW_HOMING_BIT | SW_LIMIT_BIT) // homing and limits -#define SW_MODE_MAX_VALUE SW_MODE_HOMING_LIMIT +#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 +#define SW_MODE_HOMING_LIMIT (SW_HOMING_BIT | SW_LIMIT_BIT) // homing and limits +#define SW_MODE_MAX_VALUE SW_MODE_HOMING_LIMIT enum swType { - SW_TYPE_NORMALLY_OPEN = 0, - SW_TYPE_NORMALLY_CLOSED + SW_TYPE_NORMALLY_OPEN = 0, + SW_TYPE_NORMALLY_CLOSED }; enum swState { - SW_DISABLED = -1, - SW_OPEN = 0, // also read as 'false' - SW_CLOSED // also read as 'true' + SW_DISABLED = -1, + SW_OPEN = 0, // also read as 'false' + SW_CLOSED // also read as 'true' }; -/* - * Defines for old switch handling code - */ // macros for finding the index into the switch table give the axis number #define MIN_SWITCH(axis) (axis*2) #define MAX_SWITCH(axis) (axis*2+1) -enum swDebounce { // state machine for managing debouncing and lockout - SW_IDLE = 0, - SW_DEGLITCHING, - SW_LOCKOUT +enum swDebounce { // state machine for managing debouncing and lockout + SW_IDLE = 0, + SW_DEGLITCHING, + SW_LOCKOUT }; -enum swNums { // indexes into switch arrays - SW_MIN_X = 0, - SW_MAX_X, - SW_MIN_Y, - SW_MAX_Y, - SW_MIN_Z, - SW_MAX_Z, - SW_MIN_A, - SW_MAX_A, - NUM_SWITCHES // must be last one. Used for array sizing and for loops +enum swNums { // indexes into switch arrays + SW_MIN_X = 0, + SW_MAX_X, + SW_MIN_Y, + SW_MAX_Y, + SW_MIN_Z, + SW_MAX_Z, + SW_MIN_A, + SW_MAX_A, + NUM_SWITCHES // must be last one. Used for array sizing and for loops }; -#define SW_OFFSET SW_MAX_X // offset between MIN and MAX switches +#define SW_OFFSET SW_MAX_X // offset between MIN and MAX switches #define NUM_SWITCH_PAIRS (NUM_SWITCHES/2) -/* - * Defines for new switch handling code - */ - -// switch array configuration / sizing -#define SW_PAIRS HOMING_AXES // number of axes that can have switches -#define SW_POSITIONS 2 // swPosition is either SW_MIN or SW)MAX - -enum swPosition { - SW_MIN = 0, - SW_MAX -}; - -enum swEdge { - SW_NO_EDGE = 0, - SW_LEADING, - SW_TRAILING, -}; - /* * Interrupt levels and vectors - The vectors are hard-wired to xmega ports * If you change axis port assignments you need to change these, too. */ // Interrupt level: pick one: -//#define GPIO1_INTLVL (PORT_INT0LVL_HI_gc|PORT_INT1LVL_HI_gc) // can't be hi +//#define GPIO1_INTLVL (PORT_INT0LVL_HI_gc|PORT_INT1LVL_HI_gc) // can't be hi #define GPIO1_INTLVL (PORT_INT0LVL_MED_gc|PORT_INT1LVL_MED_gc) -//#define GPIO1_INTLVL (PORT_INT0LVL_LO_gc|PORT_INT1LVL_LO_gc) // shouldn;t be low +//#define GPIO1_INTLVL (PORT_INT0LVL_LO_gc|PORT_INT1LVL_LO_gc) // shouldn;t be low // port assignments for vectors -#define X_MIN_ISR_vect PORTA_INT0_vect // these must line up with the SWITCH assignments in system.h +#define X_MIN_ISR_vect PORTA_INT0_vect // these must line up with the SWITCH assignments in system.h #define Y_MIN_ISR_vect PORTD_INT0_vect #define Z_MIN_ISR_vect PORTE_INT0_vect #define A_MIN_ISR_vect PORTF_INT0_vect @@ -134,70 +109,42 @@ enum swEdge { /* * Switch control structures -// Note 1: The term "thrown" is used because switches could be normally-open -// or normally-closed. "Thrown" means activated or hit. + // Note 1: The term "thrown" is used because switches could be normally-open + // or normally-closed. "Thrown" means activated or hit. */ -struct swStruct { // switch state - uint8_t switch_type; // 0=NO, 1=NC - applies to all switches - uint8_t limit_flag; // 1=limit switch thrown - do a lockout - uint8_t sw_num_thrown; // number of switch that was just thrown - uint8_t state[NUM_SWITCHES]; // 0=OPEN, 1=CLOSED (depends on switch type) - volatile uint8_t mode[NUM_SWITCHES]; // 0=disabled, 1=homing, 2=homing+limit, 3=limit - volatile uint8_t debounce[NUM_SWITCHES]; // switch debouncer state machine - see swDebounce - volatile int8_t count[NUM_SWITCHES]; // deglitching and lockout counter +struct swStruct { // switch state + uint8_t switch_type; // 0=NO, 1=NC - applies to all switches + uint8_t limit_flag; // 1=limit switch thrown - do a lockout + uint8_t sw_num_thrown; // number of switch that was just thrown + uint8_t state[NUM_SWITCHES]; // 0=OPEN, 1=CLOSED (depends on switch type) + volatile uint8_t mode[NUM_SWITCHES]; // 0=disabled, 1=homing, 2=homing+limit, 3=limit + volatile uint8_t debounce[NUM_SWITCHES]; // switch debouncer state machine - see swDebounce + volatile int8_t count[NUM_SWITCHES]; // deglitching and lockout counter }; struct swStruct sw; -//*** Structures from new-style switch code --- NOT YET FOLDED IN ***// - -typedef struct swSwitch { // one struct per switch - // public - uint8_t type; // swType: 0=NO, 1=NC - uint8_t mode; // 0=disabled, 1=homing, 2=limit, 3=homing+limit - uint8_t state; // set true if switch is closed - - // private - uint8_t edge; // keeps a transient record of edges for immediate inquiry - uint16_t debounce_ticks; // number of millisecond ticks for debounce lockout - uint32_t debounce_timeout; // time to expire current debounce lockout, or 0 if no lockout - void (*when_open)(struct swSwitch *s); // callback to action function when sw is open - passes *s, returns void - void (*when_closed)(struct swSwitch *s); // callback to action function when closed - void (*on_leading)(struct swSwitch *s); // callback to action function for leading edge onset - void (*on_trailing)(struct swSwitch *s); // callback to action function for trailing edge -} switch_t; -typedef void (*sw_callback)(switch_t *s); // typedef for switch action callback - -typedef struct swSwitchArray { // array of switches - uint8_t type; // switch type for entire array (default) - switch_t s[SW_PAIRS][SW_POSITIONS]; -} switches_t; - -/**************************************************************************************** - * Function prototypes - */ -void switch_init(void); + +void switch_init(); uint8_t read_switch(uint8_t sw_num); uint8_t get_switch_mode(uint8_t sw_num); -void switch_rtc_callback(void); -uint8_t get_limit_switch_thrown(void); -uint8_t get_switch_thrown(void); -void reset_switches(void); -void sw_show_switch(void); +void switch_rtc_callback(); +uint8_t get_limit_switch_thrown(); +uint8_t get_switch_thrown(); +void reset_switches(); +void sw_show_switch(); -void set_switch_type( uint8_t switch_type ); +void set_switch_type(uint8_t switch_type); uint8_t get_switch_type(); -/* - * Switch config accessors and text functions - */ +// Switch config accessors and text functions stat_t sw_set_st(nvObj_t *nv); stat_t sw_set_sw(nvObj_t *nv); #ifdef __TEXT_MODE - void sw_print_st(nvObj_t *nv); +void sw_print_st(nvObj_t *nv); #else - #define sw_print_st tx_print_stub +#define sw_print_st tx_print_stub #endif // __TEXT_MODE #endif // End of include guard: SWITCH_H_ONCE diff --git a/src/system.c b/src/system.c index 48434f8..6154f1b 100755 --- a/src/system.c +++ b/src/system.c @@ -26,9 +26,9 @@ * * ------ * Notes: - * - add full interrupt tables and dummy interrupt routine (maybe) - * - add crystal oscillator failover - * - add watchdog timer functions + * - add full interrupt tables and dummy interrupt routine (maybe) + * - add crystal oscillator failover + * - add watchdog timer functions * */ @@ -46,84 +46,84 @@ void sys_init() { - xmega_init(); // set system clock + xmega_init(); // set system clock sys_port_bindings(8); } void sys_port_bindings(float hw_version) { - device.st_port[0] = &PORT_MOTOR_1; - device.st_port[1] = &PORT_MOTOR_2; - device.st_port[2] = &PORT_MOTOR_3; - device.st_port[3] = &PORT_MOTOR_4; + device.st_port[0] = &PORT_MOTOR_1; + device.st_port[1] = &PORT_MOTOR_2; + device.st_port[2] = &PORT_MOTOR_3; + device.st_port[3] = &PORT_MOTOR_4; - device.sw_port[0] = &PORT_SWITCH_X; - device.sw_port[1] = &PORT_SWITCH_Y; - device.sw_port[2] = &PORT_SWITCH_Z; - device.sw_port[3] = &PORT_SWITCH_A; + device.sw_port[0] = &PORT_SWITCH_X; + device.sw_port[1] = &PORT_SWITCH_Y; + device.sw_port[2] = &PORT_SWITCH_Z; + device.sw_port[3] = &PORT_SWITCH_A; - if (hw_version > 6.9) { - device.out_port[0] = &PORT_OUT_V7_X; - device.out_port[1] = &PORT_OUT_V7_Y; - device.out_port[2] = &PORT_OUT_V7_Z; - device.out_port[3] = &PORT_OUT_V7_A; - } else { - device.out_port[0] = &PORT_OUT_V6_X; - device.out_port[1] = &PORT_OUT_V6_Y; - device.out_port[2] = &PORT_OUT_V6_Z; - device.out_port[3] = &PORT_OUT_V6_A; - } + if (hw_version > 6.9) { + device.out_port[0] = &PORT_OUT_V7_X; + device.out_port[1] = &PORT_OUT_V7_Y; + device.out_port[2] = &PORT_OUT_V7_Z; + device.out_port[3] = &PORT_OUT_V7_A; + } else { + device.out_port[0] = &PORT_OUT_V6_X; + device.out_port[1] = &PORT_OUT_V6_Y; + device.out_port[2] = &PORT_OUT_V6_Z; + device.out_port[3] = &PORT_OUT_V6_A; + } } uint8_t sys_read_calibration_byte(uint8_t index) { - NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; // Load NVM Command register to read the calibration row - uint8_t result = pgm_read_byte(index); - NVM_CMD = NVM_CMD_NO_OPERATION_gc; // Clean up NVM Command register - return(result); + NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; // Load NVM Command register to read the calibration row + uint8_t result = pgm_read_byte(index); + NVM_CMD = NVM_CMD_NO_OPERATION_gc; // Clean up NVM Command register + return(result); } /* * sys_get_id() - get a human readable signature * - * Produces a unique deviceID based on the factory calibration data. Format is: - * 123456-ABC + * Produces 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 - * The alpha is the lo 5 bits of wafer number and XY coords in printable ASCII - * Refer to NVM_PROD_SIGNATURES_t in iox192a3.h for details. + * The number part is a direct readout of the 6 digit lot number + * The alpha is the lo 5 bits of wafer number and XY coords in printable ASCII + * Refer to NVM_PROD_SIGNATURES_t in iox192a3.h for details. */ enum { - LOTNUM0=8, // Lot Number Byte 0, ASCII - LOTNUM1, // Lot Number Byte 1, ASCII - LOTNUM2, // Lot Number Byte 2, ASCII - LOTNUM3, // Lot Number Byte 3, ASCII - LOTNUM4, // Lot Number Byte 4, ASCII - LOTNUM5, // Lot Number Byte 5, ASCII - WAFNUM =16, // Wafer Number - COORDX0=18, // Wafer Coordinate X Byte 0 - COORDX1, // Wafer Coordinate X Byte 1 - COORDY0, // Wafer Coordinate Y Byte 0 - COORDY1, // Wafer Coordinate Y Byte 1 + LOTNUM0=8, // Lot Number Byte 0, ASCII + LOTNUM1, // Lot Number Byte 1, ASCII + LOTNUM2, // Lot Number Byte 2, ASCII + LOTNUM3, // Lot Number Byte 3, ASCII + LOTNUM4, // Lot Number Byte 4, ASCII + LOTNUM5, // Lot Number Byte 5, ASCII + WAFNUM =16, // Wafer Number + COORDX0=18, // Wafer Coordinate X Byte 0 + COORDX1, // Wafer Coordinate X Byte 1 + COORDY0, // Wafer Coordinate Y Byte 0 + COORDY1, // Wafer Coordinate Y Byte 1 }; void sys_get_id(char *id) { - char printable[33] = {"ABCDEFGHJKLMNPQRSTUVWXYZ23456789"}; - uint8_t i; + char printable[33] = {"ABCDEFGHJKLMNPQRSTUVWXYZ23456789"}; + uint8_t i; - NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; // Load NVM Command register to read the calibration row + NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; // Load NVM Command register to read the calibration row - for (i=0; i<6; i++) { - id[i] = pgm_read_byte(LOTNUM0 + i); - } - id[i++] = '-'; - id[i++] = printable[(pgm_read_byte(WAFNUM) & 0x1F)]; - id[i++] = printable[(pgm_read_byte(COORDX0) & 0x1F)]; -// id[i++] = printable[(pgm_read_byte(COORDX1) & 0x1F)]; - id[i++] = printable[(pgm_read_byte(COORDY0) & 0x1F)]; -// id[i++] = printable[(pgm_read_byte(COORDY1) & 0x1F)]; - id[i] = 0; + for (i=0; i<6; i++) { + id[i] = pgm_read_byte(LOTNUM0 + i); + } + id[i++] = '-'; + id[i++] = printable[(pgm_read_byte(WAFNUM) & 0x1F)]; + id[i++] = printable[(pgm_read_byte(COORDX0) & 0x1F)]; +// id[i++] = printable[(pgm_read_byte(COORDX1) & 0x1F)]; + id[i++] = printable[(pgm_read_byte(COORDY0) & 0x1F)]; +// id[i++] = printable[(pgm_read_byte(COORDY1) & 0x1F)]; + id[i] = 0; - NVM_CMD = NVM_CMD_NO_OPERATION_gc; // Clean up NVM Command register + NVM_CMD = NVM_CMD_NO_OPERATION_gc; // Clean up NVM Command register } diff --git a/src/system.h b/src/system.h index 395abc9..b3a1357 100755 --- a/src/system.h +++ b/src/system.h @@ -27,57 +27,52 @@ /* * INTERRUPT USAGE - TinyG uses a lot of them all over the place * - * HI Stepper DDA pulse generation (set in stepper.h) - * HI Stepper load routine SW interrupt (set in stepper.h) - * HI Dwell timer counter (set in stepper.h) - * LO Segment execution SW interrupt (set in stepper.h) - * MED GPIO1 switch port (set in gpio.h) - * MED Serial RX for USB & RS-485 (set in xio_usart.h) - * LO Serial TX for USB & RS-485 (set in xio_usart.h) - * LO Real time clock interrupt (set in xmega_rtc.h) + * HI Stepper DDA pulse generation (set in stepper.h) + * HI Stepper load routine SW interrupt (set in stepper.h) + * HI Dwell timer counter (set in stepper.h) + * LO Segment execution SW interrupt (set in stepper.h) + * MED GPIO1 switch port (set in gpio.h) + * MED Serial RX for USB & RS-485 (set in xio_usart.h) + * LO Serial TX for USB & RS-485 (set in xio_usart.h) + * LO Real time clock interrupt (set in xmega_rtc.h) */ #ifndef system_h #define system_h -void sys_init(void); // master hardware init +void sys_init(); // master hardware init void sys_port_bindings(float hw_version); void sys_get_id(char *id); -#define SYS_ID_LEN 12 // length of system ID string from sys_get_id() - -/* CPU clock */ - -#undef F_CPU // set for delays -#define F_CPU 32000000UL // should always precede +#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 +//#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 /*** 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 -#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 -#define PORT_SWITCH_Y PORTD -#define PORT_SWITCH_Z PORTE -#define PORT_SWITCH_A PORTF - -#define PORT_OUT_V7_X PORTA // v7 mapping - Output bits mapped to ports -#define PORT_OUT_V7_Y PORTF -#define PORT_OUT_V7_Z PORTD -#define PORT_OUT_V7_A PORTE - -#define PORT_OUT_V6_X PORTA // v6 and earlier mapping - Output bits mapped to ports -#define PORT_OUT_V6_Y PORTF -#define PORT_OUT_V6_Z PORTE -#define PORT_OUT_V6_A PORTD +#define PORT_MOTOR_1 PORTA // motors mapped to ports +#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 +#define PORT_SWITCH_Y PORTD +#define PORT_SWITCH_Z PORTE +#define PORT_SWITCH_A PORTF + +#define PORT_OUT_V7_X PORTA // v7 mapping - Output bits mapped to ports +#define PORT_OUT_V7_Y PORTF +#define PORT_OUT_V7_Z PORTD +#define PORT_OUT_V7_A PORTE + +#define PORT_OUT_V6_X PORTA // v6 and earlier mapping - Output bits mapped to ports +#define PORT_OUT_V6_Y PORTF +#define PORT_OUT_V6_Z PORTE +#define PORT_OUT_V6_A PORTD // These next four must be changed when the PORT_MOTOR_* definitions change! #define PORTCFG_VP0MAP_PORT_MOTOR_1_gc PORTCFG_VP0MAP_PORTA_gc @@ -85,88 +80,87 @@ void sys_get_id(char *id); #define PORTCFG_VP2MAP_PORT_MOTOR_3_gc PORTCFG_VP2MAP_PORTE_gc #define PORTCFG_VP3MAP_PORT_MOTOR_4_gc PORTCFG_VP3MAP_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 +#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) - * b3 (out) microstep 0 - * b4 (out) microstep 1 - * b5 (out) output bit for GPIO port1 - * b6 (in) min limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) - * b7 (in) max limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) + * b0 (out) step (SET is step, CLR is rest) + * b1 (out) direction (CLR = Clockwise) + * b2 (out) motor enable (CLR = Enabled) + * b3 (out) microstep 0 + * b4 (out) microstep 1 + * b5 (out) output bit for GPIO port1 + * b6 (in) min limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) + * b7 (in) max limit switch on GPIO 2 (note: motor controls and GPIO2 port mappings are not the same) */ -#define MOTOR_PORT_DIR_gm 0x3F // dir settings: lower 6 out, upper 2 in - -enum cfgPortBits { // motor control port bit positions - STEP_BIT_bp = 0, // bit 0 - DIRECTION_BIT_bp, // bit 1 - MOTOR_ENABLE_BIT_bp, // bit 2 - MICROSTEP_BIT_0_bp, // bit 3 - MICROSTEP_BIT_1_bp, // bit 4 - GPIO1_OUT_BIT_bp, // bit 5 (4 gpio1 output bits; 1 from each axis) - SW_MIN_BIT_bp, // bit 6 (4 input bits for homing/limit switches) - SW_MAX_BIT_bp // bit 7 (4 input bits for homing/limit switches) +#define MOTOR_PORT_DIR_gm 0x3F // dir settings: lower 6 out, upper 2 in + +enum cfgPortBits { // motor control port bit positions + STEP_BIT_bp = 0, // bit 0 + DIRECTION_BIT_bp, // bit 1 + MOTOR_ENABLE_BIT_bp, // bit 2 + MICROSTEP_BIT_0_bp, // bit 3 + MICROSTEP_BIT_1_bp, // bit 4 + GPIO1_OUT_BIT_bp, // bit 5 (4 gpio1 output bits; 1 from each axis) + SW_MIN_BIT_bp, // bit 6 (4 input bits for homing/limit switches) + SW_MAX_BIT_bp // bit 7 (4 input bits for homing/limit switches) }; -#define STEP_BIT_bm (1<value) { - case 0: { return (STAT_OK);} +uint8_t run_test(nvObj_t *nv) { + switch ((uint8_t)nv->value) { + case 0: return STAT_OK; #ifdef __CANNED_TESTS - - case 1: { xio_open(XIO_DEV_PGM, PGMFILE(&test_smoke),PGM_FLAGS); break;} - case 2: { xio_open(XIO_DEV_PGM, PGMFILE(&test_homing),PGM_FLAGS); break;} - case 3: { xio_open(XIO_DEV_PGM, PGMFILE(&test_squares),PGM_FLAGS); break;} - case 4: { xio_open(XIO_DEV_PGM, PGMFILE(&test_arcs),PGM_FLAGS); break;} - case 5: { xio_open(XIO_DEV_PGM, PGMFILE(&test_dwell),PGM_FLAGS); break;} - case 6: { xio_open(XIO_DEV_PGM, PGMFILE(&test_feedhold),PGM_FLAGS); break;} - case 7: { xio_open(XIO_DEV_PGM, PGMFILE(&test_Mcodes),PGM_FLAGS); break;} - case 8: { xio_open(XIO_DEV_PGM, PGMFILE(&test_json),PGM_FLAGS); break;} - case 9: { xio_open(XIO_DEV_PGM, PGMFILE(&test_inverse_time),PGM_FLAGS); break;} - case 10: { xio_open(XIO_DEV_PGM, PGMFILE(&test_rotary),PGM_FLAGS); break;} - case 11: { xio_open(XIO_DEV_PGM, PGMFILE(&test_small_moves),PGM_FLAGS); break;} - case 12: { xio_open(XIO_DEV_PGM, PGMFILE(&test_slow_moves),PGM_FLAGS); break;} - case 13: { xio_open(XIO_DEV_PGM, PGMFILE(&test_coordinate_offsets),PGM_FLAGS); break;} - case 14: { xio_open(XIO_DEV_PGM, PGMFILE(&test_microsteps),PGM_FLAGS); break;} - case 50: { xio_open(XIO_DEV_PGM, PGMFILE(&test_mudflap),PGM_FLAGS); break;} - case 51: { xio_open(XIO_DEV_PGM, PGMFILE(&test_braid),PGM_FLAGS); break;} + case 1: xio_open(XIO_DEV_PGM, PGMFILE(&test_smoke),PGM_FLAGS); break; + case 2: xio_open(XIO_DEV_PGM, PGMFILE(&test_homing),PGM_FLAGS); break; + case 3: xio_open(XIO_DEV_PGM, PGMFILE(&test_squares),PGM_FLAGS); break; + case 4: xio_open(XIO_DEV_PGM, PGMFILE(&test_arcs),PGM_FLAGS); break; + case 5: xio_open(XIO_DEV_PGM, PGMFILE(&test_dwell),PGM_FLAGS); break; + case 6: xio_open(XIO_DEV_PGM, PGMFILE(&test_feedhold),PGM_FLAGS); break; + case 7: xio_open(XIO_DEV_PGM, PGMFILE(&test_Mcodes),PGM_FLAGS); break; + case 8: xio_open(XIO_DEV_PGM, PGMFILE(&test_json),PGM_FLAGS); break; + case 9: xio_open(XIO_DEV_PGM, PGMFILE(&test_inverse_time),PGM_FLAGS); break; + case 10: xio_open(XIO_DEV_PGM, PGMFILE(&test_rotary),PGM_FLAGS); break; + case 11: xio_open(XIO_DEV_PGM, PGMFILE(&test_small_moves),PGM_FLAGS); break; + case 12: xio_open(XIO_DEV_PGM, PGMFILE(&test_slow_moves),PGM_FLAGS); break; + case 13: xio_open(XIO_DEV_PGM, PGMFILE(&test_coordinate_offsets),PGM_FLAGS); break; + case 14: xio_open(XIO_DEV_PGM, PGMFILE(&test_microsteps),PGM_FLAGS); break; + case 50: xio_open(XIO_DEV_PGM, PGMFILE(&test_mudflap),PGM_FLAGS); break; + case 51: xio_open(XIO_DEV_PGM, PGMFILE(&test_braid),PGM_FLAGS); break; #endif #ifdef __TEST_99 - case 99: { xio_open(XIO_DEV_PGM, PGMFILE(&test_99),PGM_FLAGS); break;} + case 99: xio_open(XIO_DEV_PGM, PGMFILE(&test_99),PGM_FLAGS); break; #endif - default: { - fprintf_P(stderr,PSTR("Test #%d not found\n"),(uint8_t)nv->value); - return (STAT_ERROR); - } - } - tg_set_primary_source(XIO_DEV_PGM); - return (STAT_OK); -} - -/* - * run_canned_startup() - run a string on startup - * - * Pre-load the USB RX (input) buffer with some test strings that will be called - * on startup. Be mindful of the char limit on the read buffer (RX_BUFFER_SIZE). - * It's best to create a test file for really complicated things. - */ -void run_canned_startup() // uncomment in tinyg.h if you want to run this -{ -#ifdef __CANNED_STARTUP + default: + fprintf_P(stderr,PSTR("Test #%d not found\n"),(uint8_t)nv->value); + return STAT_ERROR; + } -/* Run test 99 */ -// xio_queue_RX_string_usb("$test=99\n"); // run test file (doesn't work if text mode is disabled) -// xio_queue_RX_string_usb("{\"test\":99}\n"); // run test file -// xio_queue_RX_string_usb("{test:98}\n"); // run test file -// xio_queue_RX_string_usb("{test:99}\n"); // run test file + tg_set_primary_source(XIO_DEV_PGM); -#endif // __CANNED_STARTUP + return STAT_OK; } diff --git a/src/test.h b/src/test.h index a24148a..99ce20b 100755 --- a/src/test.h +++ b/src/test.h @@ -20,75 +20,74 @@ #define test_h uint8_t run_test(nvObj_t *nv); -void run_canned_startup(void); /***** DEBUG support ****** * - * DEBUGs are print statements you probably only want enabled during - * debugging, and then probably only for one section of code or another. + * DEBUGs are print statements you probably only want enabled during + * debugging, and then probably only for one section of code or another. * - * DEBUG logging is enabled if __DEBUG is defined. - * __DEBUG enables a set of arbitrary __dbXXXXXX defines that control - * various debug regions, e.g. __dbCONFIG to enable debugging in config.c. - * Each __dbXXXXXX pairs with a dbXXXXXX global variable used as a flag. - * Each dbXXXXXX is initialized to TRUE or FALSE at startup in main.c. - * dbXXXXXX is used as a condition to enable or disable logging. - * No varargs, so you must use the one with the right number of variables. - * A closing semicolon is not required but is recommended for style. + * DEBUG logging is enabled if __DEBUG is defined. + * __DEBUG enables a set of arbitrary __dbXXXXXX defines that control + * various debug regions, e.g. __dbCONFIG to enable debugging in config.c. + * Each __dbXXXXXX pairs with a dbXXXXXX global variable used as a flag. + * Each dbXXXXXX is initialized to TRUE or FALSE at startup in main.c. + * dbXXXXXX is used as a condition to enable or disable logging. + * No varargs, so you must use the one with the right number of variables. + * A closing semicolon is not required but is recommended for style. * - * DEBUG usage examples: - * DEBUG0(dbCONFIG, PSTR("String with no variables")); - * DEBUG1(dbCONFIG, PSTR("String with one variable: %f"), float_var); - * DEBUG2(dbCONFIG, PSTR("String with two variables: %4.2f, %d"), float_var, int_var); + * DEBUG usage examples: + * DEBUG0(dbCONFIG, PSTR("String with no variables")); + * DEBUG1(dbCONFIG, PSTR("String with one variable: %f"), float_var); + * DEBUG2(dbCONFIG, PSTR("String with two variables: %4.2f, %d"), float_var, int_var); * - * DEBUG print statements are coded so they occupy no program space if - * they are not enabled. If you also use __dbXXXX defines to enable debug - * code these will - of course - be in the code regardless. + * DEBUG print statements are coded so they occupy no program space if + * they are not enabled. If you also use __dbXXXX defines to enable debug + * code these will - of course - be in the code regardless. * - * There are also a variety of module-specific diagnostic print statements - * that are enabled or not depending on whether __DEBUG is defined + * There are also a variety of module-specific diagnostic print statements + * that are enabled or not depending on whether __DEBUG is defined */ #ifdef __DEBUG -void dump_everything(void); -void roll_over_and_die(void); +void dump_everything(); +void roll_over_and_die(); void print_scalar(const char *label, float value); void print_vector(const char *label, float vector[], uint8_t length); // global allocation of debug control variables - uint8_t dbECHO_GCODE_BLOCK; - uint8_t dbALINE_CALLED; - uint8_t dbSHOW_QUEUED_LINE; - uint8_t dbSHOW_LIMIT_SWITCH; - uint8_t dbSHOW_CONFIG_STATE; - uint8_t dbCONFIG_DEBUG_ENABLED; - uint8_t dbSHOW_LOAD_MOVE; + uint8_t dbECHO_GCODE_BLOCK; + uint8_t dbALINE_CALLED; + uint8_t dbSHOW_QUEUED_LINE; + uint8_t dbSHOW_LIMIT_SWITCH; + uint8_t dbSHOW_CONFIG_STATE; + uint8_t dbCONFIG_DEBUG_ENABLED; + uint8_t dbSHOW_LOAD_MOVE; #define DEBUG0(dbXXXXXX,msg) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg); \ - fprintf_P(stderr,PSTR("\n"));}} + fprintf_P(stderr,PSTR("DEBUG: ")); \ + fprintf_P(stderr,msg); \ + fprintf_P(stderr,PSTR("\n"));}} #define DEBUG1(dbXXXXXX,msg,a) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg,a); \ - fprintf_P(stderr,PSTR("\n"));}} + fprintf_P(stderr,PSTR("DEBUG: ")); \ + fprintf_P(stderr,msg,a); \ + fprintf_P(stderr,PSTR("\n"));}} #define DEBUG2(dbXXXXXX,msg,a,b) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg,a,b); \ - fprintf_P(stderr,PSTR("\n"));}} + fprintf_P(stderr,PSTR("DEBUG: ")); \ + fprintf_P(stderr,msg,a,b); \ + fprintf_P(stderr,PSTR("\n"));}} #define DEBUG3(dbXXXXXX,msg,a,b,c) { if (dbXXXXXX == TRUE) { \ - fprintf_P(stderr,PSTR("DEBUG: ")); \ - fprintf_P(stderr,msg,a,b,c); \ - fprintf_P(stderr,PSTR("\n"));}} + fprintf_P(stderr,PSTR("DEBUG: ")); \ + fprintf_P(stderr,msg,a,b,c); \ + fprintf_P(stderr,PSTR("\n"));}} #else #define DEBUG0(dbXXXXXX,msg) #define DEBUG1(dbXXXXXX,msg,a) #define DEBUG2(dbXXXXXX,msg,a,b) #define DEBUG3(dbXXXXXX,msg,a,b,c) -#endif // __DEBUG +#endif // __DEBUG /***** Runtime Segment Data Logger Stuff ***** * @@ -99,29 +98,29 @@ void print_vector(const char *label, float vector[], uint8_t length); // segment logger structure and index struct mpSegmentLog { - uint8_t move_state; - uint32_t linenum; - uint32_t segments; - float velocity; - float microseconds; + uint8_t move_state; + uint32_t linenum; + uint32_t segments; + float velocity; + float microseconds; }; struct mpSegmentLog sl[SEGMENT_LOGGER_MAX]; uint16_t sl_index; // function prototype and calling macro void segment_logger(uint8_t move_state, - uint32_t linenum, - uint32_t segments, - uint32_t segment_count, - float velocity, - float microseconds + uint32_t linenum, + uint32_t segments, + uint32_t segment_count, + float velocity, + float microseconds ); #define SEGMENT_LOGGER segment_logger(bf->move_state, \ - mr.linenum, mr.segments, mr.segment_count, \ - mr.segment_velocity, \ - mr.microseconds); + mr.linenum, mr.segments, mr.segment_count, \ + mr.segment_velocity, \ + mr.microseconds); #else #define SEGMENT_LOGGER -#endif // __SEGMENT_LOGGER -#endif // test_h +#endif // __SEGMENT_LOGGER +#endif // test_h diff --git a/src/tests/test_001_smoke.h b/src/tests/test_001_smoke.h index a94b959..c303d78 100755 --- a/src/tests/test_001_smoke.h +++ b/src/tests/test_001_smoke.h @@ -2,26 +2,26 @@ * test_001_smoke.h * * This test checks basic functionality: - * - motor 1 CW at full speed ~3 seconds - * - motor 1 CCW at full speed ~3 seconds - * - motor 2 CW at full speed ~3 seconds - * - motor 2 CCW at full speed ~3 seconds - * - motor 3 CW at full speed ~3 seconds - * - motor 3 CCW at full speed ~3 seconds - * - motor 4 CW at full speed ~3 seconds - * - motor 4 CCW at full speed ~3 seconds - * - all motors CW at full speed ~3 seconds - * - all motors CCW at full speed ~3 seconds - * - all motors CW at medium speed ~3 seconds - * - all motors CCW at medium speed ~3 seconds - * - all motors CW at slow speed ~3 seconds - * - all motors CCW at slow speed ~3 seconds - * - light LEDs 1,2 and 4 in sequence for about 1 second each: - * - short finishing move + * - motor 1 CW at full speed ~3 seconds + * - motor 1 CCW at full speed ~3 seconds + * - motor 2 CW at full speed ~3 seconds + * - motor 2 CCW at full speed ~3 seconds + * - motor 3 CW at full speed ~3 seconds + * - motor 3 CCW at full speed ~3 seconds + * - motor 4 CW at full speed ~3 seconds + * - motor 4 CCW at full speed ~3 seconds + * - all motors CW at full speed ~3 seconds + * - all motors CCW at full speed ~3 seconds + * - all motors CW at medium speed ~3 seconds + * - all motors CCW at medium speed ~3 seconds + * - all motors CW at slow speed ~3 seconds + * - all motors CCW at slow speed ~3 seconds + * - light LEDs 1,2 and 4 in sequence for about 1 second each: + * - short finishing move * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_smoke[] PROGMEM = "\ (MSG**** Smoke Test [v1] ****)\n\ diff --git a/src/tests/test_002_homing.h b/src/tests/test_002_homing.h index e09b26a..0cd902c 100755 --- a/src/tests/test_002_homing.h +++ b/src/tests/test_002_homing.h @@ -1,11 +1,11 @@ /* * test_002_homing.h * - * - Performs homing cycle in X, Y and Z + * - Performs homing cycle in X, Y and Z * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_homing[] PROGMEM = "\ (MSG**** Homing Test [v1] ****)\n\ diff --git a/src/tests/test_003_squares.h b/src/tests/test_003_squares.h index 9306efd..1bbd331 100755 --- a/src/tests/test_003_squares.h +++ b/src/tests/test_003_squares.h @@ -2,8 +2,8 @@ * test_003_squares.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_squares[] PROGMEM = "\ (MSG**** Squares Motion Test [v1] ****)\n\ diff --git a/src/tests/test_004_arcs.h b/src/tests/test_004_arcs.h index 57eb579..0cd3871 100755 --- a/src/tests/test_004_arcs.h +++ b/src/tests/test_004_arcs.h @@ -2,8 +2,8 @@ * test_004_arcs.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_arcs[] PROGMEM = "\ diff --git a/src/tests/test_005_dwell.h b/src/tests/test_005_dwell.h index 222738d..eacd85a 100755 --- a/src/tests/test_005_dwell.h +++ b/src/tests/test_005_dwell.h @@ -1,11 +1,11 @@ /* * test_005_dwell.h * - * Tests a 1 second dwell + * Tests a 1 second dwell * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_dwell[] PROGMEM = "\ (MSG**** Dwell Test [v1] ****)\n\ diff --git a/src/tests/test_006_feedhold.h b/src/tests/test_006_feedhold.h index fdce6e7..cc54358 100755 --- a/src/tests/test_006_feedhold.h +++ b/src/tests/test_006_feedhold.h @@ -2,8 +2,8 @@ * test_006_feedhold.h * * Notes: - * - The character array should be the same name as the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be the same name as the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_feedhold[] PROGMEM = "\ (MSG**** Feedhold Test [v1] ****)\n\ diff --git a/src/tests/test_007_Mcodes.h b/src/tests/test_007_Mcodes.h index 2fd3547..4a25c75 100755 --- a/src/tests/test_007_Mcodes.h +++ b/src/tests/test_007_Mcodes.h @@ -2,8 +2,8 @@ * test_007_Mcodes.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) * * Turn the bits on and off in sequence so you can see the LEDs light in a chain */ diff --git a/src/tests/test_008_json.h b/src/tests/test_008_json.h index 027cf52..2cc7fc4 100755 --- a/src/tests/test_008_json.h +++ b/src/tests/test_008_json.h @@ -2,8 +2,8 @@ * test_008_json.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_json[] PROGMEM= "\ {\"gc\":\"g00g17g21g40g49g80g90\"}\n\ diff --git a/src/tests/test_009_inverse_time.h b/src/tests/test_009_inverse_time.h index f3e0da9..83d2162 100755 --- a/src/tests/test_009_inverse_time.h +++ b/src/tests/test_009_inverse_time.h @@ -2,8 +2,8 @@ * test_009_inverse_time.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_inverse_time[] PROGMEM = "\ (MSG**** Inverse Time Motion Test [v1] ****)\n\ diff --git a/src/tests/test_010_rotary.h b/src/tests/test_010_rotary.h index a87e575..27ac8ca 100755 --- a/src/tests/test_010_rotary.h +++ b/src/tests/test_010_rotary.h @@ -2,8 +2,8 @@ * test_010_rotary.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_rotary[] PROGMEM = "\ (MSG**** Rotary Axis Motion Test [v1] ****)\n\ diff --git a/src/tests/test_011_small_moves.h b/src/tests/test_011_small_moves.h index 274c093..b379dbd 100755 --- a/src/tests/test_011_small_moves.h +++ b/src/tests/test_011_small_moves.h @@ -2,8 +2,8 @@ * test_011_small_moves.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_small_moves[] PROGMEM = "\ (MSG**** Test very short moves [v1] ****)\n\ diff --git a/src/tests/test_012_slow_moves.h b/src/tests/test_012_slow_moves.h index c7d038b..bab6f8b 100755 --- a/src/tests/test_012_slow_moves.h +++ b/src/tests/test_012_slow_moves.h @@ -2,8 +2,8 @@ * test_012_slow_moves.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) $si=3000\n\ g1y5\n\ x0\n\ diff --git a/src/tests/test_013_coordinate_offsets.h b/src/tests/test_013_coordinate_offsets.h index 452c365..b874fb0 100755 --- a/src/tests/test_013_coordinate_offsets.h +++ b/src/tests/test_013_coordinate_offsets.h @@ -2,8 +2,8 @@ * test_013_coordinate_offsets.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_coordinate_offsets[] PROGMEM = "\ (MSG**** Coordinate offsets test [v1] ****)\n\ diff --git a/src/tests/test_014_microsteps.h b/src/tests/test_014_microsteps.h index f0a8aba..90084c0 100755 --- a/src/tests/test_014_microsteps.h +++ b/src/tests/test_014_microsteps.h @@ -4,8 +4,8 @@ * Tests movement in 4 axes with all microatep settings. All moves should be the same length and time * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) */ const char test_microsteps[] PROGMEM = "\ (MSG**** Microstep Test [v1] ****)\n\ diff --git a/src/tests/test_050_mudflap.h b/src/tests/test_050_mudflap.h index fa69d7e..fae9a24 100755 --- a/src/tests/test_050_mudflap.h +++ b/src/tests/test_050_mudflap.h @@ -2,8 +2,8 @@ * test_011_mudflap.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) N25 G92 X0 Y0 Z0\n\ N30 X0.076 Y0.341\n\ diff --git a/src/tests/test_051_braid.h b/src/tests/test_051_braid.h index b3e8400..4fb7e4d 100755 --- a/src/tests/test_051_braid.h +++ b/src/tests/test_051_braid.h @@ -2,8 +2,8 @@ * test_051_braid.h * * Notes: - * - The character array should be derived from the filename (by convention) - * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) + * - The character array should be derived from the filename (by convention) + * - Comments are not allowed in the char array, but gcode comments are OK e.g. (g0 test) Search for ********************************* to get to the uncommented code @@ -12,7 +12,7 @@ N5 (M3)\n\ N6 G92X0.327Y-33.521Z-1.000\n\ N7 G0Z4.000\n\ N8 F1800.0\n\ -N9 G1 X0.327Y-33.521\n\ (Questionable???) +N9 G1 X0.327Y-33.521\n\ (Questionable???) N10 G1Z-1.000\n\ N11 X0.654Y-33.526\n\ diff --git a/src/text_parser.c b/src/text_parser.c index d6b1fc4..92d0cc7 100755 --- a/src/text_parser.c +++ b/src/text_parser.c @@ -33,13 +33,13 @@ #include "report.h" #include "help.h" #include "util.h" -#include "xio.h" // for ASCII char definitions +#include "xio/xio.h" // for ASCII char definitions -txtSingleton_t txt; // declare the singleton for either __TEXT_MODE setting +txtSingleton_t txt; // declare the singleton for either __TEXT_MODE setting #ifndef __TEXT_MODE -stat_t text_parser_stub(char_t *str) {return (STAT_OK);} +stat_t text_parser_stub(char_t *str) {return STAT_OK;} void text_response_stub(const stat_t status, char_t *buf) {} void text_print_list_stub (stat_t status, uint8_t flags) {} void tx_print_stub(nvObj_t *nv) {} @@ -49,98 +49,98 @@ void tx_print_stub(nvObj_t *nv) {} static stat_t _text_parser_kernal(char_t *str, nvObj_t *nv); /****************************************************************************** - * text_parser() - update a config setting from a text block (text mode) + * text_parser() - update a config setting from a text block (text mode) * _text_parser_kernal() - helper for above * * Use cases handled: - * - $xfr=1200 set a parameter (strict separators)) - * - $xfr 1200 set a parameter (relaxed separators) - * - $xfr display a parameter - * - $x display a group - * - ? generate a status report (multiline format) + * - $xfr=1200 set a parameter (strict separators)) + * - $xfr 1200 set a parameter (relaxed separators) + * - $xfr display a parameter + * - $x display a group + * - ? generate a status report (multiline format) */ stat_t text_parser(char_t *str) { - nvObj_t *nv = nv_reset_nv_list(); // returns first object in the body - stat_t status = STAT_OK; - - // trap special displays - if (str[0] == '?') { // handle status report case - sr_run_text_status_report(); - return (STAT_OK); - } - if (str[0] == 'H') { // print help screens - help_general((nvObj_t *)NULL); - return (STAT_OK); - } - - // pre-process the command - if ((str[0] == '$') && (str[1] == NUL)) { // treat a lone $ as a sys request - strcat(str,"sys"); - } - - // parse and execute the command (only processes 1 command per line) - ritorno(_text_parser_kernal(str, nv)); // run the parser to decode the command - if ((nv->valuetype == TYPE_NULL) || (nv->valuetype == TYPE_PARENT)) { - if (nv_get(nv) == STAT_COMPLETE){ // populate value, group values, or run uber-group displays - return (STAT_OK); // return for uber-group displays so they don't print twice - } - } else { // process SET and RUN commands - if (cm.machine_state == MACHINE_ALARM) - return (STAT_MACHINE_ALARMED); - status = nv_set(nv); // set (or run) single value - if (status == STAT_OK) { - nv_persist(nv); // conditionally persist depending on flags in array - } - } - nv_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); // print the results - return (status); + nvObj_t *nv = nv_reset_nv_list(); // returns first object in the body + stat_t status = STAT_OK; + + // trap special displays + if (str[0] == '?') { // handle status report case + sr_run_text_status_report(); + return STAT_OK; + } + if (str[0] == 'H') { // print help screens + help_general((nvObj_t *)0); + return STAT_OK; + } + + // pre-process the command + if ((str[0] == '$') && (str[1] == 0)) { // treat a lone $ as a sys request + strcat(str,"sys"); + } + + // parse and execute the command (only processes 1 command per line) + ritorno(_text_parser_kernal(str, nv)); // run the parser to decode the command + if ((nv->valuetype == TYPE_0) || (nv->valuetype == TYPE_PARENT)) { + if (nv_get(nv) == STAT_COMPLETE){ // populate value, group values, or run uber-group displays + return STAT_OK; // return for uber-group displays so they don't print twice + } + } else { // process SET and RUN commands + if (cm.machine_state == MACHINE_ALARM) + return STAT_MACHINE_ALARMED; + status = nv_set(nv); // set (or run) single value + if (status == STAT_OK) { + nv_persist(nv); // conditionally persist depending on flags in array + } + } + nv_print_list(status, TEXT_MULTILINE_FORMATTED, JSON_RESPONSE_FORMAT); // print the results + return status; } static stat_t _text_parser_kernal(char_t *str, nvObj_t *nv) { - char_t *rd, *wr; // read and write pointers -// char_t separators[] = {"="}; // STRICT: only separator allowed is = sign - char_t separators[] = {" =:|\t"}; // RELAXED: any separator someone might use - - // pre-process and normalize the string -// nv_reset_nv(nv); // initialize config object - nv_copy_string(nv, str); // make a copy for eventual reporting - if (*str == '$') str++; // ignore leading $ - for (rd = wr = str; *rd != NUL; rd++, wr++) { - *wr = tolower(*rd); // convert string to lower case - if (*rd == ',') { *wr = *(++rd);} // skip over commas - } - *wr = NUL; // terminate the string - - // parse fields into the nv struct - nv->valuetype = TYPE_NULL; - if ((rd = strpbrk(str, separators)) == NULL) { // no value part - strncpy(nv->token, str, TOKEN_LEN); - } else { - *rd = NUL; // terminate at end of name - strncpy(nv->token, str, TOKEN_LEN); - str = ++rd; - nv->value = strtof(str, &rd); // rd used as end pointer - if (rd != str) { - nv->valuetype = TYPE_FLOAT; - } - } - - // validate and post-process the token - if ((nv->index = nv_get_index((const char_t *)"", nv->token)) == NO_MATCH) { // get index or fail it - return (STAT_UNRECOGNIZED_NAME); - } - strcpy_P(nv->group, cfgArray[nv->index].group); // capture the group string if there is one - - // see if you need to strip the token - if (nv->group[0] != NUL) { - wr = nv->token; - rd = nv->token + strlen(nv->group); - while (*rd != NUL) { *(wr)++ = *(rd)++;} - *wr = NUL; - } - return (STAT_OK); + char_t *rd, *wr; // read and write pointers +// char_t separators[] = {"="}; // STRICT: only separator allowed is = sign + char_t separators[] = {" =:|\t"}; // RELAXED: any separator someone might use + + // pre-process and normalize the string +// nv_reset_nv(nv); // initialize config object + nv_copy_string(nv, str); // make a copy for eventual reporting + if (*str == '$') str++; // ignore leading $ + for (rd = wr = str; *rd != 0; rd++, wr++) { + *wr = tolower(*rd); // convert string to lower case + if (*rd == ',') { *wr = *(++rd);} // skip over commas + } + *wr = 0; // terminate the string + + // parse fields into the nv struct + nv->valuetype = TYPE_0; + if ((rd = strpbrk(str, separators)) == 0) { // no value part + strncpy(nv->token, str, TOKEN_LEN); + } else { + *rd = 0; // terminate at end of name + strncpy(nv->token, str, TOKEN_LEN); + str = ++rd; + nv->value = strtof(str, &rd); // rd used as end pointer + if (rd != str) { + nv->valuetype = TYPE_FLOAT; + } + } + + // validate and post-process the token + if ((nv->index = nv_get_index((const char_t *)"", nv->token)) == NO_MATCH) { // get index or fail it + return STAT_UNRECOGNIZED_NAME; + } + strcpy_P(nv->group, cfgArray[nv->index].group); // capture the group string if there is one + + // see if you need to strip the token + if (nv->group[0] != 0) { + wr = nv->token; + rd = nv->token + strlen(nv->group); + while (*rd != 0) { *(wr)++ = *(rd)++;} + *wr = 0; + } + return STAT_OK; } /************************************************************************************ @@ -151,22 +151,22 @@ static const char prompt_err[] PROGMEM = "tinyg [%s] err: %s: %s "; void text_response(const stat_t status, char_t *buf) { - if (txt.text_verbosity == TV_SILENT) return; // skip all this - - char units[] = "inch"; - if (cm_get_units_mode(MODEL) != INCHES) { strcpy(units, "mm"); } - - if ((status == STAT_OK) || (status == STAT_EAGAIN) || (status == STAT_NOOP)) { - fprintf_P(stderr, prompt_ok, units); - } else { - fprintf_P(stderr, prompt_err, units, get_status_message(status), buf); - } - nvObj_t *nv = nv_body+1; - - if (nv_get_type(nv) == NV_TYPE_MESSAGE) { - fprintf(stderr, (char *)*nv->stringp); - } - fprintf(stderr, "\n"); + if (txt.text_verbosity == TV_SILENT) return; // skip all this + + char units[] = "inch"; + if (cm_get_units_mode(MODEL) != INCHES) { strcpy(units, "mm"); } + + if ((status == STAT_OK) || (status == STAT_EAGAIN) || (status == STAT_NOOP)) { + fprintf_P(stderr, prompt_ok, units); + } else { + fprintf_P(stderr, prompt_err, units, get_status_message(status), buf); + } + nvObj_t *nv = nv_body+1; + + if (nv_get_type(nv) == NV_TYPE_MESSAGE) { + fprintf(stderr, (char *)*nv->stringp); + } + fprintf(stderr, "\n"); } /***** PRINT FUNCTIONS ******************************************************** @@ -178,73 +178,73 @@ void text_response(const stat_t status, char_t *buf) void text_print_list(stat_t status, uint8_t flags) { - switch (flags) { - case TEXT_NO_PRINT: { break; } - case TEXT_INLINE_PAIRS: { text_print_inline_pairs(nv_body); break; } - case TEXT_INLINE_VALUES: { text_print_inline_values(nv_body); break; } - case TEXT_MULTILINE_FORMATTED: { text_print_multiline_formatted(nv_body);} - } + switch (flags) { + case TEXT_NO_PRINT: { break; } + case TEXT_INLINE_PAIRS: { text_print_inline_pairs(nv_body); break; } + case TEXT_INLINE_VALUES: { text_print_inline_values(nv_body); break; } + case TEXT_MULTILINE_FORMATTED: { text_print_multiline_formatted(nv_body);} + } } void text_print_inline_pairs(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - for (uint8_t i=0; ivaluetype) { - case TYPE_PARENT: { if ((nv = nv->nx) == NULL) return; continue;} // NULL means parent with no child - case TYPE_FLOAT: { preprocess_float(nv); - fntoa(global_string_buf, nv->value, nv->precision); - fprintf_P(stderr,PSTR("%s:%s"), nv->token, global_string_buf) ; break; - } - case TYPE_INTEGER: { fprintf_P(stderr,PSTR("%s:%1.0f"), nv->token, nv->value); break;} - case TYPE_DATA: { fprintf_P(stderr,PSTR("%s:%lu"), nv->token, *v); break;} - case TYPE_STRING: { fprintf_P(stderr,PSTR("%s:%s"), nv->token, *nv->stringp); break;} - case TYPE_EMPTY: { fprintf_P(stderr,PSTR("\n")); return; } - } - if ((nv = nv->nx) == NULL) return; - if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} - } + uint32_t *v = (uint32_t*)&nv->value; + for (uint8_t i=0; ivaluetype) { + case TYPE_PARENT: { if ((nv = nv->nx) == 0) return; continue;} // 0 means parent with no child + case TYPE_FLOAT: { preprocess_float(nv); + fntoa(global_string_buf, nv->value, nv->precision); + fprintf_P(stderr,PSTR("%s:%s"), nv->token, global_string_buf) ; break; + } + case TYPE_INTEGER: { fprintf_P(stderr,PSTR("%s:%1.0f"), nv->token, nv->value); break;} + case TYPE_DATA: { fprintf_P(stderr,PSTR("%s:%lu"), nv->token, *v); break;} + case TYPE_STRING: { fprintf_P(stderr,PSTR("%s:%s"), nv->token, *nv->stringp); break;} + case TYPE_EMPTY: { fprintf_P(stderr,PSTR("\n")); return; } + } + if ((nv = nv->nx) == 0) return; + if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} + } } void text_print_inline_values(nvObj_t *nv) { - uint32_t *v = (uint32_t*)&nv->value; - for (uint8_t i=0; ivaluetype) { - case TYPE_PARENT: { if ((nv = nv->nx) == NULL) return; continue;} // NULL means parent with no child - case TYPE_FLOAT: { preprocess_float(nv); - fntoa(global_string_buf, nv->value, nv->precision); - fprintf_P(stderr,PSTR("%s"), global_string_buf) ; break; - } - case TYPE_INTEGER: { fprintf_P(stderr,PSTR("%1.0f"), nv->value); break;} - case TYPE_DATA: { fprintf_P(stderr,PSTR("%lu"), *v); break;} - case TYPE_STRING: { fprintf_P(stderr,PSTR("%s"), *nv->stringp); break;} - case TYPE_EMPTY: { fprintf_P(stderr,PSTR("\n")); return; } - } - if ((nv = nv->nx) == NULL) return; - if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} - } + uint32_t *v = (uint32_t*)&nv->value; + for (uint8_t i=0; ivaluetype) { + case TYPE_PARENT: { if ((nv = nv->nx) == 0) return; continue;} // 0 means parent with no child + case TYPE_FLOAT: { preprocess_float(nv); + fntoa(global_string_buf, nv->value, nv->precision); + fprintf_P(stderr,PSTR("%s"), global_string_buf) ; break; + } + case TYPE_INTEGER: { fprintf_P(stderr,PSTR("%1.0f"), nv->value); break;} + case TYPE_DATA: { fprintf_P(stderr,PSTR("%lu"), *v); break;} + case TYPE_STRING: { fprintf_P(stderr,PSTR("%s"), *nv->stringp); break;} + case TYPE_EMPTY: { fprintf_P(stderr,PSTR("\n")); return; } + } + if ((nv = nv->nx) == 0) return; + if (nv->valuetype != TYPE_EMPTY) { fprintf_P(stderr,PSTR(","));} + } } void text_print_multiline_formatted(nvObj_t *nv) { - for (uint8_t i=0; ivaluetype != TYPE_PARENT) { - preprocess_float(nv); - nv_print(nv); - } - if ((nv = nv->nx) == NULL) return; - if (nv->valuetype == TYPE_EMPTY) break; - } + for (uint8_t i=0; ivaluetype != TYPE_PARENT) { + preprocess_float(nv); + nv_print(nv); + } + if ((nv = nv->nx) == 0) return; + if (nv->valuetype == TYPE_EMPTY) break; + } } /* * Text print primitives using generic formats */ -static const char fmt_str[] PROGMEM = "%s\n"; // generic format for string message (with no formatting) -static const char fmt_ui8[] PROGMEM = "%d\n"; // generic format for ui8s -static const char fmt_int[] PROGMEM = "%lu\n"; // generic format for ui16's and ui32s -static const char fmt_flt[] PROGMEM = "%f\n"; // generic format for floats +static const char fmt_str[] PROGMEM = "%s\n"; // generic format for string message (with no formatting) +static const char fmt_ui8[] PROGMEM = "%d\n"; // generic format for ui8s +static const char fmt_int[] PROGMEM = "%lu\n"; // generic format for ui16's and ui32s +static const char fmt_flt[] PROGMEM = "%f\n"; // generic format for floats void tx_print_nul(nvObj_t *nv) {} void tx_print_str(nvObj_t *nv) { text_print_str(nv, fmt_str);} @@ -255,10 +255,10 @@ void tx_print_flt(nvObj_t *nv) { text_print_flt(nv, fmt_flt);} /* * Text print primitives using external formats * - * NOTE: format's are passed in as flash strings (PROGMEM) + * NOTE: format's are passed in as flash strings (PROGMEM) */ -void text_print_nul(nvObj_t *nv, const char *format) { fprintf_P(stderr, format);} // just print the format string +void text_print_nul(nvObj_t *nv, const char *format) { fprintf_P(stderr, format);} // just print the format string void text_print_str(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, *nv->stringp);} void text_print_ui8(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, (uint8_t)nv->value);} void text_print_int(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, (uint32_t)nv->value);} @@ -266,7 +266,7 @@ void text_print_flt(nvObj_t *nv, const char *format) { fprintf_P(stderr, format, void text_print_flt_units(nvObj_t *nv, const char *format, const char *units) { - fprintf_P(stderr, format, nv->value, units); + fprintf_P(stderr, format, nv->value, units); } /* diff --git a/src/text_parser.h b/src/text_parser.h index b9e3e57..2e62b3b 100755 --- a/src/text_parser.h +++ b/src/text_parser.h @@ -29,26 +29,26 @@ #define TEXT_PARSER_H_ONCE enum textVerbosity { - TV_SILENT = 0, // no response is provided - TV_VERBOSE // response is provided. Error responses ech message and failed commands + TV_SILENT = 0, // no response is provided + TV_VERBOSE // response is provided. Error responses ech message and failed commands }; -enum textFormats { // text output print modes - TEXT_NO_PRINT = 0, // don't print anything if you find yourself in TEXT mode - TEXT_INLINE_PAIRS, // print key:value pairs as comma separated pairs - TEXT_INLINE_VALUES, // print values as commas separated values - TEXT_MULTILINE_FORMATTED // print formatted values on separate lines with formatted print per line +enum textFormats { // text output print modes + TEXT_NO_PRINT = 0, // don't print anything if you find yourself in TEXT mode + TEXT_INLINE_PAIRS, // print key:value pairs as comma separated pairs + TEXT_INLINE_VALUES, // print values as commas separated values + TEXT_MULTILINE_FORMATTED // print formatted values on separate lines with formatted print per line }; -typedef struct txtSingleton { // text mode data +typedef struct txtSingleton { // text mode data - /*** config values (PUBLIC) ***/ + /*** config values (PUBLIC) ***/ - char_t format[NV_FORMAT_LEN+1]; + char_t format[NV_FORMAT_LEN+1]; - /*** runtime values (PRIVATE) ***/ + /*** runtime values (PRIVATE) ***/ - uint8_t text_verbosity; // see enum in this file for settings + uint8_t text_verbosity; // see enum in this file for settings } txtSingleton_t; extern txtSingleton_t txt; @@ -57,41 +57,41 @@ extern txtSingleton_t txt; #ifdef __TEXT_MODE - stat_t text_parser(char_t *str); - void text_response(const stat_t status, char_t *buf); - void text_print_list(stat_t status, uint8_t flags); - void text_print_inline_pairs(nvObj_t *nv); - void text_print_inline_values(nvObj_t *nv); - void text_print_multiline_formatted(nvObj_t *nv); + stat_t text_parser(char_t *str); + void text_response(const stat_t status, char_t *buf); + void text_print_list(stat_t status, uint8_t flags); + void text_print_inline_pairs(nvObj_t *nv); + void text_print_inline_values(nvObj_t *nv); + void text_print_multiline_formatted(nvObj_t *nv); - void tx_print_nul(nvObj_t *nv); - void tx_print_str(nvObj_t *nv); - void tx_print_ui8(nvObj_t *nv); - void tx_print_int(nvObj_t *nv); - void tx_print_flt(nvObj_t *nv); + void tx_print_nul(nvObj_t *nv); + void tx_print_str(nvObj_t *nv); + void tx_print_ui8(nvObj_t *nv); + void tx_print_int(nvObj_t *nv); + void tx_print_flt(nvObj_t *nv); - void text_print_nul(nvObj_t *nv, const char *format); - void text_print_str(nvObj_t *nv, const char *format); - void text_print_ui8(nvObj_t *nv, const char *format); - void text_print_int(nvObj_t *nv, const char *format); - void text_print_flt(nvObj_t *nv, const char *format); - void text_print_flt_units(nvObj_t *nv, const char *format, const char *units); + void text_print_nul(nvObj_t *nv, const char *format); + void text_print_str(nvObj_t *nv, const char *format); + void text_print_ui8(nvObj_t *nv, const char *format); + void text_print_int(nvObj_t *nv, const char *format); + void text_print_flt(nvObj_t *nv, const char *format); + void text_print_flt_units(nvObj_t *nv, const char *format, const char *units); - void tx_print_tv(nvObj_t *nv); + void tx_print_tv(nvObj_t *nv); #else - #define text_parser text_parser_stub - #define text_response text_response_stub - #define text_print_list text_print_list_stub - #define tx_print_nul tx_print_stub - #define tx_print_ui8 tx_print_stub - #define tx_print_int tx_print_stub - #define tx_print_flt tx_print_stub - #define tx_print_str tx_print_stub - #define tx_print_tv tx_print_stub - - void tx_print_stub(nvObj_t *nv); + #define text_parser text_parser_stub + #define text_response text_response_stub + #define text_print_list text_print_list_stub + #define tx_print_nul tx_print_stub + #define tx_print_ui8 tx_print_stub + #define tx_print_int tx_print_stub + #define tx_print_flt tx_print_stub + #define tx_print_str tx_print_stub + #define tx_print_tv tx_print_stub + + void tx_print_stub(nvObj_t *nv); #endif diff --git a/src/tinyg.h b/src/tinyg.h index 62a46a7..1a27405 100755 --- a/src/tinyg.h +++ b/src/tinyg.h @@ -43,41 +43,40 @@ /****** REVISIONS ******/ #ifndef TINYG_FIRMWARE_BUILD -#define TINYG_FIRMWARE_BUILD 440.20 // arc test +#define TINYG_FIRMWARE_BUILD 440.20 // arc test #endif -#define TINYG_FIRMWARE_VERSION 0.97 // firmware major version -#define TINYG_HARDWARE_PLATFORM HW_PLATFORM_TINYG_XMEGA // see hardware.h -#define TINYG_HARDWARE_VERSION HW_VERSION_TINYGV8 // see hardware.h -#define TINYG_HARDWARE_VERSION_MAX TINYG_HARDWARE_VERSION +#define TINYG_FIRMWARE_VERSION 0.97 // firmware major version +#define TINYG_HARDWARE_PLATFORM HW_PLATFORM_TINYG_XMEGA // see hardware.h +#define TINYG_HARDWARE_VERSION HW_VERSION_TINYGV8 // see hardware.h +#define TINYG_HARDWARE_VERSION_MAX TINYG_HARDWARE_VERSION /****** COMPILE-TIME SETTINGS ******/ #define __STEP_CORRECTION -//#define __NEW_SWITCHES // Using v9 style switch code -//#define __JERK_EXEC // Use computed jerk (versus forward difference based exec) -//#define __KAHAN // Use Kahan summation in aline exec functions +//#define __JERK_EXEC // Use computed jerk (versus forward difference based exec) +//#define __KAHAN // Use Kahan summation in aline exec functions -#define __TEXT_MODE // enables text mode (~10Kb) -#define __HELP_SCREENS // enables help screens (~3.5Kb) -//#define __CANNED_TESTS // enables $tests (~12Kb) -//#define __TEST_99 // enables diagnostic test 99 (independent of other tests) +#define __TEXT_MODE // enables text mode (~10Kb) +#define __HELP_SCREENS // enables help screens (~3.5Kb) +//#define __CANNED_TESTS // enables $tests (~12Kb) +//#define __TEST_99 // enables diagnostic test 99 (independent of other tests) /****** DEVELOPMENT SETTINGS ******/ -#define __DIAGNOSTIC_PARAMETERS // enables system diagnostic parameters (_xx) in config_app -//#define __DEBUG_SETTINGS // special settings. See settings.h -//#define __CANNED_STARTUP // run any canned startup moves +#define __DIAGNOSTIC_PARAMETERS // enables system diagnostic parameters (_xx) in config_app +//#define __DEBUG_SETTINGS // special settings. See settings.h +//#define __CANNED_STARTUP // run any canned startup moves -#include // defines PROGMEM and PSTR +#include // defines PROGMEM and PSTR typedef char char_t; - // gets rely on nv->index having been set -#define GET_TABLE_WORD(a) pgm_read_word(&cfgArray[nv->index].a) // get word value from cfgArray -#define GET_TABLE_BYTE(a) pgm_read_byte(&cfgArray[nv->index].a) // get byte value from cfgArray -#define GET_TABLE_FLOAT(a) pgm_read_float(&cfgArray[nv->index].a) // get float value from cfgArray -#define GET_TOKEN_BYTE(a) (char_t)pgm_read_byte(&cfgArray[i].a) // get token byte value from cfgArray + // gets rely on nv->index having been set +#define GET_TABLE_WORD(a) pgm_read_word(&cfgArray[nv->index].a) // get word value from cfgArray +#define GET_TABLE_BYTE(a) pgm_read_byte(&cfgArray[nv->index].a) // get byte value from cfgArray +#define GET_TABLE_FLOAT(a) pgm_read_float(&cfgArray[nv->index].a) // get float value from cfgArray +#define GET_TOKEN_BYTE(a) (char_t)pgm_read_byte(&cfgArray[i].a) // get token byte value from cfgArray // populate the shared buffer with the token string given the index #define GET_TOKEN_STRING(i,a) strcpy_P(a, (char *)&cfgArray[(index_t)i].token); @@ -89,50 +88,50 @@ typedef char char_t; #define GET_UNITS(a) strncpy_P(global_string_buf,(const char *)pgm_read_word(&msg_units[cm_get_units_mode(a)]), MESSAGE_LEN-1) // IO settings -#define STD_IN XIO_DEV_USB // default IO settings -#define STD_OUT XIO_DEV_USB -#define STD_ERR XIO_DEV_USB +#define STD_IN XIO_DEV_USB // default IO settings +#define STD_OUT XIO_DEV_USB +#define STD_ERR XIO_DEV_USB // String compatibility -#define strtof strtod // strtof is not in the AVR lib +#define strtof strtod // strtof is not in the AVR lib /****************************************************************************** ***** TINYG APPLICATION DEFINITIONS ****************************************** ******************************************************************************/ -typedef uint16_t magic_t; // magic number size -#define MAGICNUM 0x12EF // used for memory integrity assertions +typedef uint16_t magic_t; // magic number size +#define MAGICNUM 0x12EF // used for memory integrity assertions /***** Axes, motors & PWM channels used by the application *****/ // Axes, motors & PWM channels must be defines (not enums) so #ifdef can be used -#define AXES 6 // number of axes supported in this version -#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) -#define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems (1-6) -#define PWMS 2 // number of supported PWM channels +#define AXES 6 // number of axes supported in this version +#define HOMING_AXES 4 // number of axes that can be homed (assumes Zxyabc sequence) +#define MOTORS 4 // number of motors on the board +#define COORDS 6 // number of supported coordinate systems (1-6) +#define PWMS 2 // number of supported PWM channels // Note: If you change COORDS you must adjust the entries in cfgArray table in config.c -#define AXIS_X 0 -#define AXIS_Y 1 -#define AXIS_Z 2 -#define AXIS_A 3 -#define AXIS_B 4 -#define AXIS_C 5 -#define AXIS_U 6 // reserved -#define AXIS_V 7 // reserved -#define AXIS_W 8 // reserved - -#define MOTOR_1 0 // define motor numbers and array indexes -#define MOTOR_2 1 // must be defines. enums don't work -#define MOTOR_3 2 -#define MOTOR_4 3 -#define MOTOR_5 4 -#define MOTOR_6 5 - -#define PWM_1 0 -#define PWM_2 1 +#define AXIS_X 0 +#define AXIS_Y 1 +#define AXIS_Z 2 +#define AXIS_A 3 +#define AXIS_B 4 +#define AXIS_C 5 +#define AXIS_U 6 // reserved +#define AXIS_V 7 // reserved +#define AXIS_W 8 // reserved + +#define MOTOR_1 0 // define motor numbers and array indexes +#define MOTOR_2 1 // must be defines. enums don't work +#define MOTOR_3 2 +#define MOTOR_4 3 +#define MOTOR_5 4 +#define MOTOR_6 5 + +#define PWM_1 0 +#define PWM_2 1 /************************************************************************************ * STATUS CODES @@ -146,14 +145,14 @@ typedef uint16_t magic_t; // magic number size * * Ranges are: * - * 0 - 19 OS, communications and low-level status (must align with XIO_xxxx codes in xio.h) + * 0 - 19 OS, communications and low-level status (must align with XIO_xxxx codes in xio.h) * - * 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 @@ -165,10 +164,10 @@ typedef uint16_t magic_t; // magic number size */ typedef uint8_t stat_t; -extern stat_t status_code; // allocated in main.c +extern stat_t status_code; // allocated in main.c -#define MESSAGE_LEN 80 // global message string storage allocation -extern char global_string_buf[]; // allocated in main.c +#define MESSAGE_LEN 80 // global message string storage allocation +extern char global_string_buf[]; // allocated in main.c char *get_status_message(stat_t status); @@ -177,195 +176,195 @@ char *get_status_message(stat_t status); #define ritorno(a) if((status_code=a) != STAT_OK) { return(status_code); } // OS, communications and low-level status (must align with XIO_xxxx codes in xio.h) -#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_NOOP 3 // function had no-operation -#define STAT_COMPLETE 4 // operation is complete -#define STAT_TERMINATE 5 // operation terminated (gracefully) -#define STAT_RESET 6 // operation was hard reset (sig kill) -#define STAT_EOL 7 // function returned end-of-line -#define STAT_EOF 8 // function returned end-of-file -#define STAT_FILE_NOT_OPEN 9 -#define STAT_FILE_SIZE_EXCEEDED 10 -#define STAT_NO_SUCH_DEVICE 11 -#define STAT_BUFFER_EMPTY 12 -#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_FUNCTION_IS_STUBBED 17 -#define STAT_ERROR_18 18 -#define STAT_ERROR_19 19 // NOTE: XIO codes align to here +#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_NOOP 3 // function had no-operation +#define STAT_COMPLETE 4 // operation is complete +#define STAT_TERMINATE 5 // operation terminated (gracefully) +#define STAT_RESET 6 // operation was hard reset (sig kill) +#define STAT_EOL 7 // function returned end-of-line +#define STAT_EOF 8 // function returned end-of-file +#define STAT_FILE_NOT_OPEN 9 +#define STAT_FILE_SIZE_EXCEEDED 10 +#define STAT_NO_SUCH_DEVICE 11 +#define STAT_BUFFER_EMPTY 12 +#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_FUNCTION_IS_STUBBED 17 +#define STAT_ERROR_18 18 +#define STAT_ERROR_19 19 // NOTE: XIO codes align to here // 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_FLOATING_POINT_ERROR 22 // number conversion error -#define STAT_DIVIDE_BY_ZERO 23 -#define STAT_INVALID_ADDRESS 24 -#define STAT_READ_ONLY_ADDRESS 25 -#define STAT_INIT_FAIL 26 -#define STAT_ALARMED 27 -#define STAT_FAILED_TO_GET_PLANNER_BUFFER 28 -#define STAT_GENERIC_EXCEPTION_REPORT 29 // used for test - -#define STAT_PREP_LINE_MOVE_TIME_IS_INFINITE 30 -#define STAT_PREP_LINE_MOVE_TIME_IS_NAN 31 -#define STAT_FLOAT_IS_INFINITE 32 -#define STAT_FLOAT_IS_NAN 33 -#define STAT_PERSISTENCE_ERROR 34 -#define STAT_BAD_STATUS_REPORT_SETTING 35 -#define STAT_ERROR_36 36 -#define STAT_ERROR_37 37 -#define STAT_ERROR_38 38 -#define STAT_ERROR_39 39 - -#define STAT_ERROR_40 40 -#define STAT_ERROR_41 41 -#define STAT_ERROR_42 42 -#define STAT_ERROR_43 43 -#define STAT_ERROR_44 44 -#define STAT_ERROR_45 45 -#define STAT_ERROR_46 46 -#define STAT_ERROR_47 47 -#define STAT_ERROR_48 48 -#define STAT_ERROR_49 49 - -#define STAT_ERROR_50 50 -#define STAT_ERROR_51 51 -#define STAT_ERROR_52 52 -#define STAT_ERROR_53 53 -#define STAT_ERROR_54 54 -#define STAT_ERROR_55 55 -#define STAT_ERROR_56 56 -#define STAT_ERROR_57 57 -#define STAT_ERROR_58 58 -#define STAT_ERROR_59 59 - -#define STAT_ERROR_60 60 -#define STAT_ERROR_61 61 -#define STAT_ERROR_62 62 -#define STAT_ERROR_63 63 -#define STAT_ERROR_64 64 -#define STAT_ERROR_65 65 -#define STAT_ERROR_66 66 -#define STAT_ERROR_67 67 -#define STAT_ERROR_68 68 -#define STAT_ERROR_69 69 - -#define STAT_ERROR_70 70 -#define STAT_ERROR_71 71 -#define STAT_ERROR_72 72 -#define STAT_ERROR_73 73 -#define STAT_ERROR_74 74 -#define STAT_ERROR_75 75 -#define STAT_ERROR_76 76 -#define STAT_ERROR_77 77 -#define STAT_ERROR_78 78 -#define STAT_ERROR_79 79 - -#define STAT_ERROR_80 80 -#define STAT_ERROR_81 81 -#define STAT_ERROR_82 82 -#define STAT_ERROR_83 83 -#define STAT_ERROR_84 84 -#define STAT_ERROR_85 85 -#define STAT_ERROR_86 86 -#define STAT_ERROR_87 87 -#define STAT_ERROR_88 88 -#define STAT_ERROR_89 89 +#define STAT_INTERNAL_ERROR 20 // unrecoverable internal error +#define STAT_INTERNAL_RANGE_ERROR 21 // number range other than by user input +#define STAT_FLOATING_POINT_ERROR 22 // number conversion error +#define STAT_DIVIDE_BY_ZERO 23 +#define STAT_INVALID_ADDRESS 24 +#define STAT_READ_ONLY_ADDRESS 25 +#define STAT_INIT_FAIL 26 +#define STAT_ALARMED 27 +#define STAT_FAILED_TO_GET_PLANNER_BUFFER 28 +#define STAT_GENERIC_EXCEPTION_REPORT 29 // used for test + +#define STAT_PREP_LINE_MOVE_TIME_IS_INFINITE 30 +#define STAT_PREP_LINE_MOVE_TIME_IS_NAN 31 +#define STAT_FLOAT_IS_INFINITE 32 +#define STAT_FLOAT_IS_NAN 33 +#define STAT_PERSISTENCE_ERROR 34 +#define STAT_BAD_STATUS_REPORT_SETTING 35 +#define STAT_ERROR_36 36 +#define STAT_ERROR_37 37 +#define STAT_ERROR_38 38 +#define STAT_ERROR_39 39 + +#define STAT_ERROR_40 40 +#define STAT_ERROR_41 41 +#define STAT_ERROR_42 42 +#define STAT_ERROR_43 43 +#define STAT_ERROR_44 44 +#define STAT_ERROR_45 45 +#define STAT_ERROR_46 46 +#define STAT_ERROR_47 47 +#define STAT_ERROR_48 48 +#define STAT_ERROR_49 49 + +#define STAT_ERROR_50 50 +#define STAT_ERROR_51 51 +#define STAT_ERROR_52 52 +#define STAT_ERROR_53 53 +#define STAT_ERROR_54 54 +#define STAT_ERROR_55 55 +#define STAT_ERROR_56 56 +#define STAT_ERROR_57 57 +#define STAT_ERROR_58 58 +#define STAT_ERROR_59 59 + +#define STAT_ERROR_60 60 +#define STAT_ERROR_61 61 +#define STAT_ERROR_62 62 +#define STAT_ERROR_63 63 +#define STAT_ERROR_64 64 +#define STAT_ERROR_65 65 +#define STAT_ERROR_66 66 +#define STAT_ERROR_67 67 +#define STAT_ERROR_68 68 +#define STAT_ERROR_69 69 + +#define STAT_ERROR_70 70 +#define STAT_ERROR_71 71 +#define STAT_ERROR_72 72 +#define STAT_ERROR_73 73 +#define STAT_ERROR_74 74 +#define STAT_ERROR_75 75 +#define STAT_ERROR_76 76 +#define STAT_ERROR_77 77 +#define STAT_ERROR_78 78 +#define STAT_ERROR_79 79 + +#define STAT_ERROR_80 80 +#define STAT_ERROR_81 81 +#define STAT_ERROR_82 82 +#define STAT_ERROR_83 83 +#define STAT_ERROR_84 84 +#define STAT_ERROR_85 85 +#define STAT_ERROR_86 86 +#define STAT_ERROR_87 87 +#define STAT_ERROR_88 88 +#define STAT_ERROR_89 89 // Assertion failures - build down from 99 until they meet the system internal errors -#define STAT_CONFIG_ASSERTION_FAILURE 90 -#define STAT_XIO_ASSERTION_FAILURE 91 -#define STAT_ENCODER_ASSERTION_FAILURE 92 -#define STAT_STEPPER_ASSERTION_FAILURE 93 -#define STAT_PLANNER_ASSERTION_FAILURE 94 -#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 detected by magic numbers -#define STAT_GENERIC_ASSERTION_FAILURE 99 // generic assertion failure - unclassified +#define STAT_CONFIG_ASSERTION_FAILURE 90 +#define STAT_XIO_ASSERTION_FAILURE 91 +#define STAT_ENCODER_ASSERTION_FAILURE 92 +#define STAT_STEPPER_ASSERTION_FAILURE 93 +#define STAT_PLANNER_ASSERTION_FAILURE 94 +#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 detected by magic numbers +#define STAT_GENERIC_ASSERTION_FAILURE 99 // generic assertion failure - unclassified // 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_ERROR_114 114 -#define STAT_ERROR_115 115 -#define STAT_ERROR_116 116 -#define STAT_ERROR_117 117 -#define STAT_ERROR_118 118 -#define STAT_ERROR_119 119 - -#define STAT_ERROR_120 120 -#define STAT_ERROR_121 121 -#define STAT_ERROR_122 122 -#define STAT_ERROR_123 123 -#define STAT_ERROR_124 124 -#define STAT_ERROR_125 125 -#define STAT_ERROR_126 126 -#define STAT_ERROR_127 127 -#define STAT_ERROR_128 128 -#define STAT_ERROR_129 129 +#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_ERROR_114 114 +#define STAT_ERROR_115 115 +#define STAT_ERROR_116 116 +#define STAT_ERROR_117 117 +#define STAT_ERROR_118 118 +#define STAT_ERROR_119 119 + +#define STAT_ERROR_120 120 +#define STAT_ERROR_121 121 +#define STAT_ERROR_122 122 +#define STAT_ERROR_123 123 +#define STAT_ERROR_124 124 +#define STAT_ERROR_125 125 +#define STAT_ERROR_126 126 +#define STAT_ERROR_127 127 +#define STAT_ERROR_128 128 +#define STAT_ERROR_129 129 // 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 // 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_REQUESTED_VELOCITY_EXCEEDS_LIMITS 146 #define STAT_CUTTER_COMPENSATION_CANNOT_BE_ENABLED 147 #define STAT_PROGRAMMED_POINT_SAME_AS_CURRENT_POINT 148 -#define STAT_SPINDLE_SPEED_BELOW_MINIMUM 149 - -#define STAT_SPINDLE_SPEED_MAX_EXCEEDED 150 -#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_SPINDLE_SPEED_BELOW_MINIMUM 149 + +#define STAT_SPINDLE_SPEED_MAX_EXCEEDED 150 +#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_ARC_RADIUS_OUT_OF_TOLERANCE 158 // WARNING - radius arc is too small or too large - accuracy in question #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 // 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_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 // 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_NOT_VALID_TOOL_NUMBER 165 #define STAT_D_WORD_IS_MISSING 166 #define STAT_D_WORD_IS_INVALID 167 @@ -383,88 +382,88 @@ char *get_status_message(stat_t status); #define STAT_T_WORD_IS_MISSING 178 #define STAT_T_WORD_IS_INVALID 179 -#define STAT_ERROR_180 180 // reserved for Gcode errors -#define STAT_ERROR_181 181 -#define STAT_ERROR_182 182 -#define STAT_ERROR_183 183 -#define STAT_ERROR_184 184 -#define STAT_ERROR_185 185 -#define STAT_ERROR_186 186 -#define STAT_ERROR_187 187 -#define STAT_ERROR_188 188 -#define STAT_ERROR_189 189 - -#define STAT_ERROR_190 190 -#define STAT_ERROR_191 191 -#define STAT_ERROR_192 192 -#define STAT_ERROR_193 193 -#define STAT_ERROR_194 194 -#define STAT_ERROR_195 195 -#define STAT_ERROR_196 196 -#define STAT_ERROR_197 197 -#define STAT_ERROR_198 198 -#define STAT_ERROR_199 199 +#define STAT_ERROR_180 180 // reserved for Gcode errors +#define STAT_ERROR_181 181 +#define STAT_ERROR_182 182 +#define STAT_ERROR_183 183 +#define STAT_ERROR_184 184 +#define STAT_ERROR_185 185 +#define STAT_ERROR_186 186 +#define STAT_ERROR_187 187 +#define STAT_ERROR_188 188 +#define STAT_ERROR_189 189 + +#define STAT_ERROR_190 190 +#define STAT_ERROR_191 191 +#define STAT_ERROR_192 192 +#define STAT_ERROR_193 193 +#define STAT_ERROR_194 194 +#define STAT_ERROR_195 195 +#define STAT_ERROR_196 196 +#define STAT_ERROR_197 197 +#define STAT_ERROR_198 198 +#define STAT_ERROR_199 199 // 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_ERROR_206 206 -#define STAT_ERROR_207 207 -#define STAT_ERROR_208 208 -#define STAT_ERROR_209 209 - -#define STAT_ERROR_210 210 -#define STAT_ERROR_211 211 -#define STAT_ERROR_212 212 -#define STAT_ERROR_213 213 -#define STAT_ERROR_214 214 -#define STAT_ERROR_215 215 -#define STAT_ERROR_216 216 -#define STAT_ERROR_217 217 -#define STAT_ERROR_218 218 -#define STAT_ERROR_219 219 - -#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_ERROR_233 233 -#define STAT_ERROR_234 234 -#define STAT_ERROR_235 235 -#define STAT_ERROR_236 236 -#define STAT_ERROR_237 237 -#define STAT_ERROR_238 238 -#define STAT_ERROR_239 239 - -#define STAT_HOMING_CYCLE_FAILED 240 // homing cycle did not complete -#define STAT_HOMING_ERROR_BAD_OR_NO_AXIS 241 -#define STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY 242 -#define STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY 243 -#define STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL 244 -#define STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF 245 -#define STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION 246 -#define STAT_ERROR_247 247 -#define STAT_ERROR_248 248 -#define STAT_ERROR_249 249 - -#define STAT_PROBE_CYCLE_FAILED 250 // probing 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. 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_ERROR_206 206 +#define STAT_ERROR_207 207 +#define STAT_ERROR_208 208 +#define STAT_ERROR_209 209 + +#define STAT_ERROR_210 210 +#define STAT_ERROR_211 211 +#define STAT_ERROR_212 212 +#define STAT_ERROR_213 213 +#define STAT_ERROR_214 214 +#define STAT_ERROR_215 215 +#define STAT_ERROR_216 216 +#define STAT_ERROR_217 217 +#define STAT_ERROR_218 218 +#define STAT_ERROR_219 219 + +#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_ERROR_233 233 +#define STAT_ERROR_234 234 +#define STAT_ERROR_235 235 +#define STAT_ERROR_236 236 +#define STAT_ERROR_237 237 +#define STAT_ERROR_238 238 +#define STAT_ERROR_239 239 + +#define STAT_HOMING_CYCLE_FAILED 240 // homing cycle did not complete +#define STAT_HOMING_ERROR_BAD_OR_NO_AXIS 241 +#define STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY 242 +#define STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY 243 +#define STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL 244 +#define STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF 245 +#define STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION 246 +#define STAT_ERROR_247 247 +#define STAT_ERROR_248 248 +#define STAT_ERROR_249 249 + +#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 +#define STAT_JOGGING_CYCLE_FAILED 252 // jogging cycle did not complete // !!! Do not exceed 255 without also changing stat_t typedef diff --git a/src/util.c b/src/util.c index 8dc4338..b583c83 100755 --- a/src/util.c +++ b/src/util.c @@ -26,8 +26,8 @@ */ /* util contains a dog's breakfast of supporting functions that are not specific to tinyg: * including: - * - math and min/max utilities and extensions - * - vector manipulation utilities + * - math and min/max utilities and extensions + * - vector manipulation utilities */ #include "tinyg.h" @@ -36,200 +36,200 @@ #include "xmega/xmega_rtc.h" /**** Vector utilities **** - * copy_vector() - copy vector of arbitrary length - * vector_equal() - test if vectors are equal - * get_axis_vector_length() - return the length of an axis vector - * set_vector() - load values into vector form - * set_vector_by_axis() - load a single value into a zero vector + * copy_vector() - copy vector of arbitrary length + * vector_equal() - test if vectors are equal + * get_axis_vector_length() - return the length of an axis vector + * set_vector() - load values into vector form + * set_vector_by_axis() - load a single value into a zero vector */ -float vector[AXES]; // statically allocated global for vector utilities +float vector[AXES]; // statically allocated global for vector utilities 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); + 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; } float get_axis_vector_length(const float a[], const float b[]) { - return (sqrt(square(a[AXIS_X] - b[AXIS_X]) + - square(a[AXIS_Y] - b[AXIS_Y]) + - square(a[AXIS_Z] - b[AXIS_Z]) + - square(a[AXIS_A] - b[AXIS_A]) + - square(a[AXIS_B] - b[AXIS_B]) + - square(a[AXIS_C] - b[AXIS_C]))); + return sqrt(square(a[AXIS_X] - b[AXIS_X] + + square(a[AXIS_Y] - b[AXIS_Y]) + + square(a[AXIS_Z] - b[AXIS_Z]) + + square(a[AXIS_A] - b[AXIS_A]) + + square(a[AXIS_B] - b[AXIS_B]) + + square(a[AXIS_C] - b[AXIS_C]))); } float *set_vector(float x, float y, float z, float a, float b, float c) { - vector[AXIS_X] = x; - vector[AXIS_Y] = y; - vector[AXIS_Z] = z; - vector[AXIS_A] = a; - vector[AXIS_B] = b; - vector[AXIS_C] = c; - return (vector); + vector[AXIS_X] = x; + vector[AXIS_Y] = y; + vector[AXIS_Z] = z; + vector[AXIS_A] = a; + vector[AXIS_B] = b; + vector[AXIS_C] = c; + return 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; - } - return (vector); + 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; + } + return vector; } /**** Math and other general purpose functions ****/ /* Slightly faster (*) multi-value min and max functions - * min3() - return minimum of 3 numbers - * min4() - return minimum of 4 numbers - * max3() - return maximum of 3 numbers - * max4() - return maximum of 4 numbers + * min3() - return minimum of 3 numbers + * min4() - return minimum of 4 numbers + * max3() - return maximum of 3 numbers + * max4() - return maximum of 4 numbers * * Implementation tip: Order the min and max values from most to least likely in the calling args * * (*) Macro min4 is about 20uSec, inline function version is closer to 10 uSec (Xmega 32 MHz) - * #define min3(a,b,c) (min(min(a,b),c)) - * #define min4(a,b,c,d) (min(min(a,b),min(c,d))) - * #define max3(a,b,c) (max(max(a,b),c)) - * #define max4(a,b,c,d) (max(max(a,b),max(c,d))) + * #define min3(a,b,c) (min(min(a,b),c)) + * #define min4(a,b,c,d) (min(min(a,b),min(c,d))) + * #define max3(a,b,c) (max(max(a,b),c)) + * #define max4(a,b,c,d) (max(max(a,b),max(c,d))) */ float min3(float x1, float x2, float x3) { - float min = x1; - if (x2 < min) { min = x2;} - if (x3 < min) { return (x3);} - return (min); + float min = x1; + if (x2 < min) { min = x2;} + if (x3 < min) { return x3;} + return min; } float min4(float x1, float x2, float x3, float x4) { - float min = x1; - if (x2 < min) { min = x2;} - if (x3 < min) { min = x3;} - if (x4 < min) { return (x4);} - return (min); + float min = x1; + if (x2 < min) { min = x2;} + if (x3 < min) { min = x3;} + if (x4 < min) { return x4;} + return min; } float max3(float x1, float x2, float x3) { - float max = x1; - if (x2 > max) { max = x2;} - if (x3 > max) { return (x3);} - return (max); + float max = x1; + if (x2 > max) { max = x2;} + if (x3 > max) { return x3;} + return max; } float max4(float x1, float x2, float x3, float x4) { - float max = x1; - if (x2 > max) { max = x2;} - if (x3 > max) { max = x3;} - if (x4 > max) { return (x4);} - return (max); + float max = x1; + if (x2 > max) { max = x2;} + if (x3 > max) { max = x3;} + if (x4 > max) { return x4;} + return max; } /**** String utilities **** - * strcpy_U() - strcpy workalike to get around initial NUL for blank string - possibly wrong - * isnumber() - isdigit that also accepts plus, minus, and decimal point + * strcpy_U() - strcpy workalike to get around initial 0 for blank string - possibly wrong + * isnumber() - isdigit that also accepts plus, minus, and decimal point * escape_string() - add escapes to a string - currently for quotes only */ uint8_t isnumber(char_t c) { - if (c == '.') { return (true); } - if (c == '-') { return (true); } - if (c == '+') { return (true); } - return (isdigit(c)); + if (c == '.') { return true; } + if (c == '-') { return true; } + if (c == '+') { return true; } + return isdigit(c); } char_t *escape_string(char_t *dst, char_t *src) { - char_t c; - char_t *start_dst = dst; - - while ((c = *(src++)) != 0) { // NUL - if (c == '"') { *(dst++) = '\\'; } - *(dst++) = c; - } - return (start_dst); + char_t c; + char_t *start_dst = dst; + + while ((c = *(src++)) != 0) { // 0 + if (c == '"') { *(dst++) = '\\'; } + *(dst++) = c; + } + return start_dst; } /* * pstr2str() - return an AVR style progmem string as a RAM string. * - * This function copies a string from FLASH to a pre-allocated RAM buffer - + * This function copies a string from FLASH to a pre-allocated RAM buffer - * see main.c for allocation and max length. */ char_t *pstr2str(const char *pgm_string) { strncpy_P(global_string_buf, pgm_string, MESSAGE_LEN); - return (global_string_buf); + return global_string_buf; } /* * fntoa() - return ASCII string given a float and a decimal precision value * - * Returns length of string, less the terminating NUL character + * Returns length of string, less the terminating 0 character */ char_t fntoa(char_t *str, float n, uint8_t precision) { // handle special cases - if (isnan(n)) { - strcpy(str, "nan"); - return (3); - - } else if (isinf(n)) { - strcpy(str, "inf"); - return (3); - - } else if (precision == 0 ) { return((char_t)sprintf((char *)str, "%0.0f", (double) n)); - } else if (precision == 1 ) { return((char_t)sprintf((char *)str, "%0.1f", (double) n)); - } else if (precision == 2 ) { return((char_t)sprintf((char *)str, "%0.2f", (double) n)); - } else if (precision == 3 ) { return((char_t)sprintf((char *)str, "%0.3f", (double) n)); - } else if (precision == 4 ) { return((char_t)sprintf((char *)str, "%0.4f", (double) n)); - } else if (precision == 5 ) { return((char_t)sprintf((char *)str, "%0.5f", (double) n)); - } else if (precision == 6 ) { return((char_t)sprintf((char *)str, "%0.6f", (double) n)); - } else if (precision == 7 ) { return((char_t)sprintf((char *)str, "%0.7f", (double) n)); - } else { return((char_t)sprintf((char *)str, "%f", (double) n)); } + if (isnan(n)) { + strcpy(str, "nan"); + return 3; + + } else if (isinf(n)) { + strcpy(str, "inf"); + return 3; + + } else if (precision == 0 ) { return((char_t)sprintf((char *)str, "%0.0f", (double) n)); + } else if (precision == 1 ) { return((char_t)sprintf((char *)str, "%0.1f", (double) n)); + } else if (precision == 2 ) { return((char_t)sprintf((char *)str, "%0.2f", (double) n)); + } else if (precision == 3 ) { return((char_t)sprintf((char *)str, "%0.3f", (double) n)); + } else if (precision == 4 ) { return((char_t)sprintf((char *)str, "%0.4f", (double) n)); + } else if (precision == 5 ) { return((char_t)sprintf((char *)str, "%0.5f", (double) n)); + } else if (precision == 6 ) { return((char_t)sprintf((char *)str, "%0.6f", (double) n)); + } else if (precision == 7 ) { return((char_t)sprintf((char *)str, "%0.7f", (double) n)); + } else { return((char_t)sprintf((char *)str, "%f", (double) n)); } } /* * compute_checksum() - calculate the checksum for a string * - * Stops calculation on null termination or length value if non-zero. + * Stops calculation on null termination or length value if non-zero. * - * This is based on the the Java hashCode function. - * See http://en.wikipedia.org/wiki/Java_hashCode() + * This is based on the the Java hashCode function. + * See http://en.wikipedia.org/wiki/Java_hashCode() */ #define HASHMASK 9999 uint16_t compute_checksum(char_t const *string, const uint16_t length) { - uint32_t h = 0; - uint16_t len = strlen(string); - if (length != 0) len = min(len, length); + uint32_t h = 0; + uint16_t len = strlen(string); + if (length != 0) len = min(len, length); for (uint16_t i=0; itermB ? termA:termB; }) + termA>termB ? termA:termB; }) #endif #ifndef min #define min(a,b) \ - ({ __typeof__ (a) term1 = (a); \ - __typeof__ (b) term2 = (b); \ - term1 EPSILON) // requires math.h to be included in each file used +#define fp_NE(a,b) (fabs(a-b) > EPSILON) // requires math.h to be included in each file used #endif #ifndef fp_ZERO -#define fp_ZERO(a) (fabs(a) < EPSILON) // requires math.h to be included in each file used +#define fp_ZERO(a) (fabs(a) < EPSILON) // requires math.h to be included in each file used #endif #ifndef fp_NOT_ZERO -#define fp_NOT_ZERO(a) (fabs(a) > EPSILON) // requires math.h to be included in each file used +#define fp_NOT_ZERO(a) (fabs(a) > EPSILON) // requires math.h to be included in each file used #endif #ifndef fp_FALSE -#define fp_FALSE(a) (a < EPSILON) // float is interpreted as FALSE (equals zero) +#define fp_FALSE(a) (a < EPSILON) // float is interpreted as FALSE (equals zero) #endif #ifndef fp_TRUE -#define fp_TRUE(a) (a > EPSILON) // float is interpreted as TRUE (not equal to zero) +#define fp_TRUE(a) (a > EPSILON) // float is interpreted as TRUE (not equal to zero) #endif // Constants @@ -131,4 +131,4 @@ uint32_t SysTickTimer_getValue(void); #define M_SQRT3 (1.73205080756888) #endif -#endif // End of include guard: UTIL_H_ONCE +#endif // End of include guard: UTIL_H_ONCE diff --git a/src/xio.c b/src/xio.c deleted file mode 100755 index bd536b0..0000000 --- a/src/xio.c +++ /dev/null @@ -1,304 +0,0 @@ -/* - * xio.c - Xmega IO devices - common code file - * 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 . - * - * 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. - */ -/* ----- XIO - Xmega Device System ---- - * - * XIO provides common access to native and derived xmega devices (see table below) - * XIO devices are compatible with avr-gcc stdio and also provide some special functions - * that are not found in stdio. - * - * Stdio support: - * - http://www.nongnu.org/avr-libc/user-manual/group__avr__stdio.html - * - Stdio compatible putc() and getc() functions provided for each device - * - This enables fgets, printf, scanf, and other stdio functions - * - Full support for formatted printing is provided (including floats) - * - Assignment of a default device to stdin, stdout & stderr is provided - * - printf() and printf_P() send to stdout, so use fprintf() to stderr - * for things that should't go over RS485 in SLAVE mode - * - * Facilities provided beyond stdio: - * - Supported devices include: - * - USB (derived from USART) - * - RS485 (derived from USART) - * - SPI devices and slave channels - * - Program memory "files" (read only) - * - Stdio FILE streams are managed as bindings to the above devices - * - Additional functions provided include: - * - open() - initialize parameters, addresses and flags - * - gets() - non-blocking input line reader - extends fgets - * - ctrl() - ioctl-like knockoff for setting device parameters (flags) - * - signal handling: interrupt on: feedhold, cycle_start, ctrl-x software reset - * - interrupt buffered RX and TX functions - * - XON/XOFF software flow control - */ -/* ----- XIO - Some Internals ---- - * - * XIO layers are: (1) xio virtual device (root), (2) xio device type, (3) xio devices - * - * The virtual device has the following methods: - * xio_init() - initialize the entire xio system - * xio_open() - open a device indicated by the XIO_DEV number - * xio_ctrl() - set control flags for XIO_DEV device - * xio_gets() - get a string from the XIO_DEV device (non blocking line reader) - * xio_getc() - read a character from the XIO_DEV device (not stdio compatible) - * xio_putc() - write a character to the XIO_DEV device (not stdio compatible) - * xio_set_baud() - set baud rates for devices for which this is meaningful - * - * The device type layer currently knows about USARTS, SPI, and File devices. Methods are: - * xio_init_() - initializes the devices of that type - * - * The device layer currently supports: USB, RS485, SPI channels, PGM file reading. methods: - * xio_open() - set up the device for use or reset the device - * xio_ctrl() - change device flag controls - * xio_gets() - get a string from the device (non-blocking) - * xio_getc() - read a character from the device (stdio compatible) - * xio_putc() - write a character to the device (stdio compatible) - * - * The virtual level uses XIO_DEV_xxx numeric device IDs for reference. - * Lower layers are called using the device structure pointer xioDev_t *d - * The stdio compatible functions use pointers to the stdio FILE structs. - */ -#include // for memset() -#include // precursor for xio.h -#include // precursor for xio.h - -#include "xio.h" // all device includes are nested here -#include "tinyg.h" // needed by init() for default source -#include "config.h" // needed by init() for default source -#include "controller.h" // needed by init() for default source - -// -typedef struct xioSingleton { - FILE * stderr_shadow; // used for stack overflow / memory integrity checking -} xioSingleton_t; -xioSingleton_t xio; - -/******************************************************************************** - * XIO Initializations, Resets and Assertions - */ -/* - * xio_init() - initialize entire xio sub-system - */ -void xio_init() -{ - // set memory integrity check - xio_set_stderr(0); // set a bogus value; may be overwritten with a real value - - // setup device types - xio_init_usart(); - xio_init_spi(); - xio_init_file(); - - // open individual devices (file device opens occur at time-of-use) - xio_open(XIO_DEV_USB, 0, USB_FLAGS); - xio_open(XIO_DEV_RS485,0, RS485_FLAGS); - xio_open(XIO_DEV_SPI1, 0, SPI_FLAGS); - xio_open(XIO_DEV_SPI2, 0, SPI_FLAGS); - - xio_init_assertions(); -} - -/* - * xio_init_assertions() - * xio_test_assertions() - validate operating state - * - * NOTE: xio device assertions are set up as part of xio_open_generic() - * This system is kind of brittle right now because if a device is - * not set up then it will fail in the assertions test. Need to fix this. - */ - -void xio_init_assertions() {} - -uint8_t xio_test_assertions() -{ - if (ds[XIO_DEV_USB].magic_start != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (ds[XIO_DEV_USB].magic_end != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (ds[XIO_DEV_RS485].magic_start != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (ds[XIO_DEV_RS485].magic_end != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (ds[XIO_DEV_SPI1].magic_start != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (ds[XIO_DEV_SPI1].magic_end != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (ds[XIO_DEV_SPI2].magic_start != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (ds[XIO_DEV_SPI2].magic_end != MAGICNUM) return (STAT_XIO_ASSERTION_FAILURE); - if (stderr != xio.stderr_shadow) return (STAT_XIO_ASSERTION_FAILURE); - return (STAT_OK); -} - -/* - * xio_isbusy() - return TRUE if XIO sub-system is busy - * - * This function is here so that the caller can detect that the serial system is active - * and therefore generating interrupts. This is a hack for the earlier AVRs that require - * interrupts to be disabled for EEPROM write so the caller can see if the XIO system is - * quiescent. This is used by the G10 deferred writeback persistence functions. - * - * Idle conditions: - * - The serial RX buffer is empty, indicating with some probability that data is not being sent - * - The serial TX buffers are empty - */ - -uint8_t xio_isbusy() -{ - if (xio_get_rx_bufcount_usart(&USBu) != 0) return (false); - if (xio_get_tx_bufcount_usart(&USBu) != 0) return (false); - return (true); -} - -/* - * xio_reset_working_flags() - */ - -void xio_reset_working_flags(xioDev_t *d) -{ - d->signal = 0; - d->flag_in_line = 0; - d->flag_eol = 0; - d->flag_eof = 0; -} - -/* - * xio_init_device() - generic initialization function for any device - * - * This binds the main fucntions and sets up the stdio FILE structure - * udata is used to point back to the device struct so it can be gotten - * from getc() and putc() functions. - * - * Requires device open() to be run prior to using the device - */ -void xio_open_generic(uint8_t dev, x_open_t x_open, - x_ctrl_t x_ctrl, - x_gets_t x_gets, - x_getc_t x_getc, - x_putc_t x_putc, - x_flow_t x_flow) -{ - xioDev_t *d = &ds[dev]; - memset (d, 0, sizeof(xioDev_t)); - d->magic_start = MAGICNUM; - d->magic_end = MAGICNUM; - d->dev = dev; - - // bind functions to device structure - d->x_open = x_open; - d->x_ctrl = x_ctrl; - d->x_gets = x_gets; - d->x_getc = x_getc; // you don't need to bind getc & putc unless you are going to use them directly - d->x_putc = x_putc; // they are bound into the fdev stream struct - d->x_flow = x_flow; - - // setup the stdio FILE struct and link udata back to the device struct - fdev_setup_stream(&d->file, x_putc, x_getc, _FDEV_SETUP_RW); - fdev_set_udata(&d->file, d); // reference yourself for udata -} - -/******************************************************************************** - * PUBLIC ENTRY POINTS - access the functions via the XIO_DEV number - * xio_open() - open function - * xio_gets() - entry point for non-blocking get line function - * xio_getc() - entry point for getc (not stdio compatible) - * xio_putc() - entry point for putc (not stdio compatible) - * - * It might be prudent to run an assertion such as below, but we trust the callers: - * if (dev < XIO_DEV_COUNT) blah blah blah - * else return (_FDEV_ERR); // XIO_NO_SUCH_DEVICE - */ -FILE *xio_open(uint8_t dev, const char *addr, flags_t flags) -{ - return (ds[dev].x_open(dev, addr, flags)); -} - -int xio_gets(const uint8_t dev, char *buf, const int size) -{ - return (ds[dev].x_gets(&ds[dev], buf, size)); -} - -int xio_getc(const uint8_t dev) -{ - return (ds[dev].x_getc(&ds[dev].file)); -} - -int xio_putc(const uint8_t dev, const char c) -{ - return (ds[dev].x_putc(c, &ds[dev].file)); -} - -/* - * xio_ctrl() - PUBLIC set control flags (top-level XIO_DEV access) - * xio_ctrl_generic() - PRIVATE but generic set-control-flags - */ -int xio_ctrl(const uint8_t dev, const flags_t flags) -{ - return (xio_ctrl_generic(&ds[dev], flags)); -} - -#define SETFLAG(t,f) if ((flags & t) != 0) { d->f = true; } -#define CLRFLAG(t,f) if ((flags & t) != 0) { d->f = false; } - -int xio_ctrl_generic(xioDev_t *d, const flags_t flags) -{ - SETFLAG(XIO_BLOCK, flag_block); - CLRFLAG(XIO_NOBLOCK, flag_block); - SETFLAG(XIO_XOFF, flag_xoff); - CLRFLAG(XIO_NOXOFF, flag_xoff); - SETFLAG(XIO_ECHO, flag_echo); - CLRFLAG(XIO_NOECHO, flag_echo); - SETFLAG(XIO_CRLF, flag_crlf); - CLRFLAG(XIO_NOCRLF, flag_crlf); - SETFLAG(XIO_IGNORECR, flag_ignorecr); - CLRFLAG(XIO_NOIGNORECR, flag_ignorecr); - SETFLAG(XIO_IGNORELF, flag_ignorelf); - CLRFLAG(XIO_NOIGNORELF, flag_ignorelf); - SETFLAG(XIO_LINEMODE, flag_linemode); - CLRFLAG(XIO_NOLINEMODE, flag_linemode); - return (XIO_OK); -} - -/* - * xio_set_baud() - PUBLIC entry to set baud rate - * Currently this only works on USART devices - */ -int xio_set_baud(const uint8_t dev, const uint8_t baud) -{ - xioUsart_t *dx = (xioUsart_t *)&us[dev - XIO_DEV_USART_OFFSET]; - xio_set_baud_usart(dx, baud); - return (XIO_OK); -} - -/* - * xio_fc_null() - flow control null function - */ -void xio_fc_null(xioDev_t *d) -{ - return; -} - -/* - * xio_set_stdin() - set stdin from device number - * xio_set_stdout() - set stdout from device number - * xio_set_stderr() - set stderr from device number - * - * stderr is defined in stdio as __iob[2]. Turns out stderr is the last RAM - * allocated by the linker for this project. We usae that to keep a shadow - * of __iob[2] for stack overflow detection and other memory corruption. - */ -void xio_set_stdin(const uint8_t dev) { stdin = &ds[dev].file; } -void xio_set_stdout(const uint8_t dev) { stdout = &ds[dev].file; } -void xio_set_stderr(const uint8_t dev) -{ - stderr = &ds[dev].file; - xio.stderr_shadow = stderr; // this is the last thing in RAM, so we use it as a memory corruption canary -} diff --git a/src/xio.h b/src/xio.h deleted file mode 100755 index 4ad9e79..0000000 --- a/src/xio.h +++ /dev/null @@ -1,398 +0,0 @@ -/* - * xio.h - Xmega IO devices - common header file - * Part of TinyG project - * - * Copyright (c) 2010 - 2014 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 . - * - * 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. - */ -/* XIO devices are compatible with avr-gcc stdio, so formatted printing - * is supported. To use this sub-system outside of TinyG you may need - * some defines in tinyg.h. See notes at end of this file for more details. - */ -/* Note: anything that includes xio.h first needs the following: - * #include // needed for FILE def'n - * #include // needed for true and false - * #include // defines prog_char, PSTR - */ -/* Note: This file contains load of sub-includes near the middle - * #include "xio_file.h" - * #include "xio_usart.h" - * #include "xio_spi.h" - * #include "xio_signals.h" - * (possibly more) - */ -/* - * CAVEAT EMPTOR: File under "watch your ass": - * - * - Short story: Do not call ANYTHING that can print (i.e. send chars to the TX - * buffer) from a medium or hi interrupt. This obviously includes any printf() - * function, but also exception reports, cm_soft_alarm(), cm_hard_alarm() and a - * few other functions that call stdio print functions. - * - * - Longer Story: The stdio printf() functions use character drivers provided by - * tinyg to access the low-level Xmega devices. Specifically xio_putc_usb() in xio_usb.c, - * and xio_putc_rs485() in xio_rs485.c. Since stdio does not understand non-blocking - * IO these functions must block if there is no space in the TX buffer. Blocking is - * accomplished using sleep_mode(). The IO system is the only place where sleep_mode() - * is used. Everything else in TinyG is non-blocking. Sleep is woken (exited) whenever - * any interrupt fires. So there must always be a viable interrupt source running when - * you enter a sleep or the system will hang (lock up). In the IO functions this is the - * TX interupts, which fire when space becomes available in the USART for a TX char. This - * Means you cannot call a print function at or above the level of the TX interrupts, - * which are set to medium. - */ -#ifndef XIO_H_ONCE -#define XIO_H_ONCE - -/************************************************************************* - * Device configurations - *************************************************************************/ -// Pre-allocated XIO devices (configured devices) -// Unused devices are commented out. All this needs to line up. - -enum xioDevNum_t { // TYPE: DEVICE: - XIO_DEV_USB, // USART USB device - XIO_DEV_RS485, // USART RS485 device - XIO_DEV_SPI1, // SPI SPI channel #1 - XIO_DEV_SPI2, // SPI SPI channel #2 -// XIO_DEV_SPI3, // SPI SPI channel #3 -// XIO_DEV_SPI4, // SPI SPI channel #4 - XIO_DEV_PGM, // FILE Program memory file (read only) -// XIO_DEV_SD, // FILE SD card (not implemented) - XIO_DEV_COUNT // total device count (must be last entry) -}; -// If your change these ^, check these v - -#define XIO_DEV_USART_COUNT 2 // # of USART devices -#define XIO_DEV_USART_OFFSET 0 // offset for computing indices - -#define XIO_DEV_SPI_COUNT 2 // # of SPI devices -#define XIO_DEV_SPI_OFFSET XIO_DEV_USART_COUNT // offset for computing indicies - -#define XIO_DEV_FILE_COUNT 1 // # of FILE devices -#define XIO_DEV_FILE_OFFSET (XIO_DEV_USART_COUNT + XIO_DEV_SPI_COUNT) // index into FILES - -// Fast accessors -#define USB ds[XIO_DEV_USB] -#define USBu us[XIO_DEV_USB - XIO_DEV_USART_OFFSET] - -/****************************************************************************** - * Device structures - * - * Each device has 3 structs. The generic device struct is declared below. - * It embeds a stdio stream struct "FILE". The FILE struct uses the udata - * field to back-reference the generic struct so getc & putc can get at it. - * Lastly there's an 'x' struct which contains data specific to each dev type. - * - * The generic open() function sets up the generic struct and the FILE stream. - * the device opens() set up the extended struct and bind it ot the generic. - ******************************************************************************/ -// NOTE" "FILE *" is another way of saying "struct __file *" -// NOTE: using the "x_" prefix fo avoid collisions with stdio defined getc, putc, etc - -#define flags_t uint16_t - -typedef struct xioDEVICE { // common device struct (one per dev) - // references and self references - uint16_t magic_start; // memory integrity check - uint8_t dev; // self referential device number - FILE file; // stdio FILE stream structure - void *x; // extended device struct binding (static) - - // function bindings - FILE *(*x_open)(const uint8_t dev, const char *addr, const flags_t flags); - int (*x_ctrl)(struct xioDEVICE *d, const flags_t flags); // set device control flags - int (*x_gets)(struct xioDEVICE *d, char *buf, const int size);// non-blocking line reader - int (*x_getc)(FILE *); // read char (stdio compatible) - int (*x_putc)(char, FILE *); // write char (stdio compatible) - void (*x_flow)(struct xioDEVICE *d); // flow control callback function - - // device configuration flags - uint8_t flag_block; - uint8_t flag_echo; - uint8_t flag_crlf; - uint8_t flag_ignorecr; - uint8_t flag_ignorelf; - uint8_t flag_linemode; - uint8_t flag_xoff; // xon/xoff enabled - - // private working data and runtime flags - int size; // text buffer length (dynamic) - uint8_t len; // chars read so far (buf array index) - uint8_t signal; // signal value - uint8_t flag_in_line; // used as a state variable for line reads - uint8_t flag_eol; // end of line detected - uint8_t flag_eof; // end of file detected - char *buf; // text buffer binding (can be dynamic) - uint16_t magic_end; -} xioDev_t; - -typedef FILE *(*x_open_t)(const uint8_t dev, const char *addr, const flags_t flags); -typedef int (*x_ctrl_t)(xioDev_t *d, const flags_t flags); -typedef int (*x_gets_t)(xioDev_t *d, char *buf, const int size); -typedef int (*x_getc_t)(FILE *); -typedef int (*x_putc_t)(char, FILE *); -typedef void (*x_flow_t)(xioDev_t *d); - -/************************************************************************* - * Sub-Includes and static allocations - *************************************************************************/ -// Put all sub-includes here so only xio.h is needed elsewhere -#include "xio/xio_file.h" -#include "xio/xio_usart.h" -#include "xio/xio_spi.h" - -// Static structure allocations -xioDev_t ds[XIO_DEV_COUNT]; // allocate top-level dev structs -xioUsart_t us[XIO_DEV_USART_COUNT]; // USART extended IO structs -xioSpi_t spi[XIO_DEV_SPI_COUNT]; // SPI extended IO structs -xioFile_t fs[XIO_DEV_FILE_COUNT]; // FILE extended IO structs -extern struct controllerSingleton tg; // needed by init() for default source - -/************************************************************************* - * Function Prototypes and Macros - *************************************************************************/ - -// Advance RX or TX head or tail. Buffers count down, so advance is a decrement. -// The zero condition is the wrap that sets the index back to the top. -#define advance_buffer(buf,len) { if ((--(buf)) == 0) buf = len-1;} - -// public functions (virtual class) -void xio_init(void); -void xio_init_assertions(void); -uint8_t xio_test_assertions(void); -uint8_t xio_isbusy(void); - -void xio_reset_working_flags(xioDev_t *d); -FILE *xio_open(const uint8_t dev, const char *addr, const flags_t flags); -int xio_ctrl(const uint8_t dev, const flags_t flags); -int xio_gets(const uint8_t dev, char *buf, const int size); -int xio_getc(const uint8_t dev); -int xio_putc(const uint8_t dev, const char c); -int xio_set_baud(const uint8_t dev, const uint8_t baud_rate); - -// generic functions (private, but at virtual level) -int xio_ctrl_generic(xioDev_t *d, const flags_t flags); - -void xio_open_generic(uint8_t dev, x_open_t x_open, - x_ctrl_t x_ctrl, - x_gets_t x_gets, - x_getc_t x_getc, - x_putc_t x_putc, - x_flow_t x_flow); - -void xio_fc_null(xioDev_t *d); // NULL flow control callback -void xio_fc_usart(xioDev_t *d); // XON/XOFF flow control callback - -// std devices -void xio_init_stdio(void); // set std devs & do startup prompt -void xio_set_stdin(const uint8_t dev); -void xio_set_stdout(const uint8_t dev); -void xio_set_stderr(const uint8_t dev); - -/************************************************************************* - * SUPPORTING DEFINTIONS - SHOULD NOT NEED TO CHANGE - *************************************************************************/ -/* - * xio control flag values - * - * if using 32 bits must cast 1 to uint32_t for bit evaluations to work correctly - * #define XIO_BLOCK ((uint32_t)1<<1) // 32 bit example. Change flags_t to uint32_t - */ - -#define XIO_BLOCK ((uint16_t)1<<0) // enable blocking reads -#define XIO_NOBLOCK ((uint16_t)1<<1) // disable blocking reads -#define XIO_XOFF ((uint16_t)1<<2) // enable XON/OFF flow control -#define XIO_NOXOFF ((uint16_t)1<<3) // disable XON/XOFF flow control -#define XIO_ECHO ((uint16_t)1<<4) // echo reads from device to stdio -#define XIO_NOECHO ((uint16_t)1<<5) // disable echo -#define XIO_CRLF ((uint16_t)1<<6) // convert to on writes -#define XIO_NOCRLF ((uint16_t)1<<7) // do not convert to on writes -#define XIO_IGNORECR ((uint16_t)1<<8) // ignore on reads -#define XIO_NOIGNORECR ((uint16_t)1<<9) // don't ignore on reads -#define XIO_IGNORELF ((uint16_t)1<<10) // ignore on reads -#define XIO_NOIGNORELF ((uint16_t)1<<11) // don't ignore on reads -#define XIO_LINEMODE ((uint16_t)1<<12) // special read handling -#define XIO_NOLINEMODE ((uint16_t)1<<13) // no special read handling - -/* - * Generic XIO signals and error conditions. - * See signals.h for application specific signal defs and routines. - */ - -enum xioSignals { - XIO_SIG_OK, // OK - XIO_SIG_EAGAIN, // would block - XIO_SIG_EOL, // end-of-line encountered (string has data) - XIO_SIG_EOF, // end-of-file encountered (string has no data) - XIO_SIG_OVERRUN, // buffer overrun - XIO_SIG_RESET, // cancel operation immediately - XIO_SIG_FEEDHOLD, // pause operation - XIO_SIG_CYCLE_START, // start or resume operation - XIO_SIG_QUEUE_FLUSH, // flush planner queue - XIO_SIG_DELETE, // backspace or delete character (BS, DEL) - XIO_SIG_BELL, // BELL character (BEL, ^g) - XIO_SIG_BOOTLOADER // ESC character - start bootloader -}; - -/* Some useful ASCII definitions */ - -#define NUL (char)0x00 // ASCII NUL char (0) (not "NULL" which is a pointer) -#define STX (char)0x02 // ^b - STX -#define ETX (char)0x03 // ^c - ETX -#define ENQ (char)0x05 // ^e - ENQuire -#define BEL (char)0x07 // ^g - BEL -#define BS (char)0x08 // ^h - backspace -#define TAB (char)0x09 // ^i - character -#define LF (char)0x0A // ^j - line feed -#define VT (char)0x0B // ^k - kill stop -#define CR (char)0x0D // ^m - carriage return -#define XON (char)0x11 // ^q - DC1, XON, resume -#define XOFF (char)0x13 // ^s - DC3, XOFF, pause -#define SYN (char)0x16 // ^v - SYN - Used for queue flush -#define CAN (char)0x18 // ^x - Cancel, abort -#define ESC (char)0x1B // ^[ - ESC(ape) -//#define SP (char)0x20 // ' ' Space character // defined externally -#define DEL (char)0x7F // DEL(ete) - -#define Q_EMPTY (char)0xFF // signal no character - -/* Signal character mappings */ - -#define CHAR_RESET CAN -#define CHAR_FEEDHOLD (char)'!' -#define CHAR_CYCLE_START (char)'~' -#define CHAR_QUEUE_FLUSH (char)'%' - -/* XIO return codes - * These codes are the "inner nest" for the STAT_ return codes. - * The first N TG codes correspond directly to these codes. - * This eases using XIO by itself (without tinyg) and simplifes using - * tinyg codes with no mapping when used together. This comes at the cost - * of making sure these lists are aligned. STAT_should be based on this list. - */ - -enum xioCodes { - XIO_OK = 0, // OK - ALWAYS ZERO - XIO_ERR, // generic error return (errors start here) - XIO_EAGAIN, // function would block here (must be called again) - XIO_NOOP, // function had no-operation - XIO_COMPLETE, // operation complete - XIO_TERMINATE, // operation terminated (gracefully) - XIO_RESET, // operation reset (ungraceful) - XIO_EOL, // function returned end-of-line - XIO_EOF, // function returned end-of-file - XIO_FILE_NOT_OPEN, // file is not open - XIO_FILE_SIZE_EXCEEDED, // maximum file size exceeded - XIO_NO_SUCH_DEVICE, // illegal or unavailable device - XIO_BUFFER_EMPTY, // more of a statement of fact than an error code - XIO_BUFFER_FULL, - XIO_BUFFER_FULL_FATAL, - XIO_INITIALIZING, // system initializing, not ready for use - XIO_ERROR_16, // reserved - XIO_ERROR_17, // reserved - XIO_ERROR_18, // reserved - XIO_ERROR_19 // NOTE: XIO codes align to here -}; -#define XIO_ERRNO_MAX XIO_BUFFER_FULL_NON_FATAL - - - -/* ASCII characters used by Gcode or otherwise unavailable for special use. - See NIST sections 3.3.2.2, 3.3.2.3 and Appendix E for Gcode uses. - See http://www.json.org/ for JSON notation - - hex char name used by: - ---- ---- ---------- -------------------- - 0x00 NUL null everything - 0x01 SOH ctrl-A - 0x02 STX ctrl-B Kinen SPI protocol - 0x03 ETX ctrl-C Kinen SPI protocol - 0x04 EOT ctrl-D - 0x05 ENQ ctrl-E - 0x06 ACK ctrl-F - 0x07 BEL ctrl-G - 0x08 BS ctrl-H - 0x09 HT ctrl-I - 0x0A LF ctrl-J - 0x0B VT ctrl-K - 0x0C FF ctrl-L - 0x0D CR ctrl-M - 0x0E SO ctrl-N - 0x0F SI ctrl-O - 0x10 DLE ctrl-P - 0x11 DC1 ctrl-Q XOFF - 0x12 DC2 ctrl-R - 0x13 DC3 ctrl-S XON - 0x14 DC4 ctrl-T - 0x15 NAK ctrl-U - 0x16 SYN ctrl-V - 0x17 ETB ctrl-W - 0x18 CAN ctrl-X TinyG / grbl software reset - 0x19 EM ctrl-Y - 0x1A SUB ctrl-Z - 0x1B ESC ctrl-[ - 0x1C FS ctrl-\ - 0x1D GS ctrl-] - 0x1E RS ctrl-^ - 0x1F US ctrl-_ - - 0x20 Gcode blocks - 0x21 ! excl point TinyG feedhold (trapped and removed from serial stream) - 0x22 " quote JSON notation - 0x23 # number Gcode parameter prefix; JSON topic prefix character - 0x24 $ dollar TinyG / grbl out-of-cycle settings prefix - 0x25 & ampersand universal symbol for logical AND (not used here) - 0x26 % percent Queue Flush character (trapped and removed from serial stream) - Also sometimes used as a file-start and file-end character in Gcode files - 0x27 ' single quote - 0x28 ( open paren Gcode comments - 0x29 ) close paren Gcode comments - 0x2A * asterisk Gcode expressions; JSON wildcard character - 0x2B + plus Gcode numbers, parameters and expressions - 0x2C , comma JSON notation - 0x2D - minus Gcode numbers, parameters and expressions - 0x2E . period Gcode numbers, parameters and expressions - 0x2F / fwd slash Gcode expressions & block delete char - 0x3A : colon JSON notation - 0x3B ; semicolon Gcode comemnt delimiter (alternate) - 0x3C < less than Gcode expressions - 0x3D = equals Gcode expressions - 0x3E > greaterthan Gcode expressions - 0x3F ? question mk TinyG / grbl query - 0x40 @ at symbol JSON address prefix character - - 0x5B [ open bracketGcode expressions - 0x5C \ backslash JSON notation (escape) - 0x5D ] close brack Gcode expressions - 0x5E ^ caret Reserved for TinyG in-cycle command prefix - 0x5F _ underscore - - 0x60 ` grave accnt - 0x7B { open curly JSON notation - 0x7C | pipe universal symbol for logical OR (not used here) - 0x7D } close curly JSON notation - 0x7E ~ tilde TinyG cycle start (trapped and removed from serial stream) - 0x7F DEL -*/ - -#endif // end of include guard: XIO_H_ONCE diff --git a/src/xio/xio.c b/src/xio/xio.c new file mode 100755 index 0000000..351a8b5 --- /dev/null +++ b/src/xio/xio.c @@ -0,0 +1,304 @@ +/* + * xio.c - Xmega IO devices - common code file + * 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 . + * + * 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. + */ +/* ----- XIO - Xmega Device System ---- + * + * XIO provides common access to native and derived xmega devices (see table below) + * XIO devices are compatible with avr-gcc stdio and also provide some special functions + * that are not found in stdio. + * + * Stdio support: + * - http://www.nongnu.org/avr-libc/user-manual/group__avr__stdio.html + * - Stdio compatible putc() and getc() functions provided for each device + * - This enables fgets, printf, scanf, and other stdio functions + * - Full support for formatted printing is provided (including floats) + * - Assignment of a default device to stdin, stdout & stderr is provided + * - printf() and printf_P() send to stdout, so use fprintf() to stderr + * for things that should't go over RS485 in SLAVE mode + * + * Facilities provided beyond stdio: + * - Supported devices include: + * - USB (derived from USART) + * - RS485 (derived from USART) + * - SPI devices and slave channels + * - Program memory "files" (read only) + * - Stdio FILE streams are managed as bindings to the above devices + * - Additional functions provided include: + * - open() - initialize parameters, addresses and flags + * - gets() - non-blocking input line reader - extends fgets + * - ctrl() - ioctl-like knockoff for setting device parameters (flags) + * - signal handling: interrupt on: feedhold, cycle_start, ctrl-x software reset + * - interrupt buffered RX and TX functions + * - XON/XOFF software flow control + */ +/* ----- XIO - Some Internals ---- + * + * XIO layers are: (1) xio virtual device (root), (2) xio device type, (3) xio devices + * + * The virtual device has the following methods: + * xio_init() - initialize the entire xio system + * xio_open() - open a device indicated by the XIO_DEV number + * xio_ctrl() - set control flags for XIO_DEV device + * xio_gets() - get a string from the XIO_DEV device (non blocking line reader) + * xio_getc() - read a character from the XIO_DEV device (not stdio compatible) + * xio_putc() - write a character to the XIO_DEV device (not stdio compatible) + * xio_set_baud() - set baud rates for devices for which this is meaningful + * + * The device type layer currently knows about USARTS, SPI, and File devices. Methods are: + * xio_init_() - initializes the devices of that type + * + * The device layer currently supports: USB, RS485, SPI channels, PGM file reading. methods: + * xio_open() - set up the device for use or reset the device + * xio_ctrl() - change device flag controls + * xio_gets() - get a string from the device (non-blocking) + * xio_getc() - read a character from the device (stdio compatible) + * xio_putc() - write a character to the device (stdio compatible) + * + * The virtual level uses XIO_DEV_xxx numeric device IDs for reference. + * Lower layers are called using the device structure pointer xioDev_t *d + * The stdio compatible functions use pointers to the stdio FILE structs. + */ +#include // for memset() +#include // precursor for xio.h +#include // precursor for xio.h + +#include "xio.h" // all device includes are nested here +#include "../tinyg.h" // needed by init() for default source +#include "../config.h" // needed by init() for default source +#include "../controller.h" // needed by init() for default source + + +typedef struct xioSingleton { + FILE * stderr_shadow; // used for stack overflow / memory integrity checking +} xioSingleton_t; +xioSingleton_t xio; + +/******************************************************************************** + * XIO Initializations, Resets and Assertions + */ +/* + * xio_init() - initialize entire xio sub-system + */ +void xio_init() +{ + // set memory integrity check + xio_set_stderr(0); // set a bogus value; may be overwritten with a real value + + // setup device types + xio_init_usart(); + xio_init_spi(); + xio_init_file(); + + // open individual devices (file device opens occur at time-of-use) + xio_open(XIO_DEV_USB, 0, USB_FLAGS); + xio_open(XIO_DEV_RS485,0, RS485_FLAGS); + xio_open(XIO_DEV_SPI1, 0, SPI_FLAGS); + xio_open(XIO_DEV_SPI2, 0, SPI_FLAGS); + + xio_init_assertions(); +} + +/* + * xio_init_assertions() + * xio_test_assertions() - validate operating state + * + * NOTE: xio device assertions are set up as part of xio_open_generic() + * This system is kind of brittle right now because if a device is + * not set up then it will fail in the assertions test. Need to fix this. + */ + +void xio_init_assertions() {} + +uint8_t xio_test_assertions() +{ + if (ds[XIO_DEV_USB].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (ds[XIO_DEV_USB].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (ds[XIO_DEV_RS485].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (ds[XIO_DEV_RS485].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (ds[XIO_DEV_SPI1].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (ds[XIO_DEV_SPI1].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (ds[XIO_DEV_SPI2].magic_start != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (ds[XIO_DEV_SPI2].magic_end != MAGICNUM) return STAT_XIO_ASSERTION_FAILURE; + if (stderr != xio.stderr_shadow) return STAT_XIO_ASSERTION_FAILURE; + return STAT_OK; +} + +/* + * xio_isbusy() - return TRUE if XIO sub-system is busy + * + * This function is here so that the caller can detect that the serial system is active + * and therefore generating interrupts. This is a hack for the earlier AVRs that require + * interrupts to be disabled for EEPROM write so the caller can see if the XIO system is + * quiescent. This is used by the G10 deferred writeback persistence functions. + * + * Idle conditions: + * - The serial RX buffer is empty, indicating with some probability that data is not being sent + * - The serial TX buffers are empty + */ + +uint8_t xio_isbusy() +{ + if (xio_get_rx_bufcount_usart(&USBu) != 0) return false; + if (xio_get_tx_bufcount_usart(&USBu) != 0) return false; + return true; +} + +/* + * xio_reset_working_flags() + */ + +void xio_reset_working_flags(xioDev_t *d) +{ + d->signal = 0; + d->flag_in_line = 0; + d->flag_eol = 0; + d->flag_eof = 0; +} + +/* + * xio_init_device() - generic initialization function for any device + * + * This binds the main fucntions and sets up the stdio FILE structure + * udata is used to point back to the device struct so it can be gotten + * from getc() and putc() functions. + * + * Requires device open() to be run prior to using the device + */ +void xio_open_generic(uint8_t dev, x_open_t x_open, + x_ctrl_t x_ctrl, + x_gets_t x_gets, + x_getc_t x_getc, + x_putc_t x_putc, + x_flow_t x_flow) +{ + xioDev_t *d = &ds[dev]; + memset (d, 0, sizeof(xioDev_t)); + d->magic_start = MAGICNUM; + d->magic_end = MAGICNUM; + d->dev = dev; + + // bind functions to device structure + d->x_open = x_open; + d->x_ctrl = x_ctrl; + d->x_gets = x_gets; + d->x_getc = x_getc; // you don't need to bind getc & putc unless you are going to use them directly + d->x_putc = x_putc; // they are bound into the fdev stream struct + d->x_flow = x_flow; + + // setup the stdio FILE struct and link udata back to the device struct + fdev_setup_stream(&d->file, x_putc, x_getc, _FDEV_SETUP_RW); + fdev_set_udata(&d->file, d); // reference yourself for udata +} + +/******************************************************************************** + * PUBLIC ENTRY POINTS - access the functions via the XIO_DEV number + * xio_open() - open function + * xio_gets() - entry point for non-blocking get line function + * xio_getc() - entry point for getc (not stdio compatible) + * xio_putc() - entry point for putc (not stdio compatible) + * + * It might be prudent to run an assertion such as below, but we trust the callers: + * if (dev < XIO_DEV_COUNT) blah blah blah + * else return _FDEV_ERR; // XIO_NO_SUCH_DEVICE + */ +FILE *xio_open(uint8_t dev, const char *addr, flags_t flags) +{ + return ds[dev].x_open(dev, addr, flags); +} + +int xio_gets(const uint8_t dev, char *buf, const int size) +{ + return ds[dev].x_gets(&ds[dev], buf, size); +} + +int xio_getc(const uint8_t dev) +{ + return ds[dev].x_getc(&ds[dev].file); +} + +int xio_putc(const uint8_t dev, const char c) +{ + return ds[dev].x_putc(c, &ds[dev].file); +} + +/* + * xio_ctrl() - PUBLIC set control flags (top-level XIO_DEV access) + * xio_ctrl_generic() - PRIVATE but generic set-control-flags + */ +int xio_ctrl(const uint8_t dev, const flags_t flags) +{ + return xio_ctrl_generic(&ds[dev], flags); +} + +#define SETFLAG(t,f) if ((flags & t) != 0) { d->f = true; } +#define CLRFLAG(t,f) if ((flags & t) != 0) { d->f = false; } + +int xio_ctrl_generic(xioDev_t *d, const flags_t flags) +{ + SETFLAG(XIO_BLOCK, flag_block); + CLRFLAG(XIO_NOBLOCK, flag_block); + SETFLAG(XIO_XOFF, flag_xoff); + CLRFLAG(XIO_NOXOFF, flag_xoff); + SETFLAG(XIO_ECHO, flag_echo); + CLRFLAG(XIO_NOECHO, flag_echo); + SETFLAG(XIO_CRLF, flag_crlf); + CLRFLAG(XIO_NOCRLF, flag_crlf); + SETFLAG(XIO_IGNORECR, flag_ignorecr); + CLRFLAG(XIO_NOIGNORECR, flag_ignorecr); + SETFLAG(XIO_IGNORELF, flag_ignorelf); + CLRFLAG(XIO_NOIGNORELF, flag_ignorelf); + SETFLAG(XIO_LINEMODE, flag_linemode); + CLRFLAG(XIO_NOLINEMODE, flag_linemode); + return XIO_OK; +} + +/* + * xio_set_baud() - PUBLIC entry to set baud rate + * Currently this only works on USART devices + */ +int xio_set_baud(const uint8_t dev, const uint8_t baud) +{ + xioUsart_t *dx = (xioUsart_t *)&us[dev - XIO_DEV_USART_OFFSET]; + xio_set_baud_usart(dx, baud); + return XIO_OK; +} + +/* + * xio_fc_null() - flow control null function + */ +void xio_fc_null(xioDev_t *d) +{ + return; +} + +/* + * xio_set_stdin() - set stdin from device number + * xio_set_stdout() - set stdout from device number + * xio_set_stderr() - set stderr from device number + * + * stderr is defined in stdio as __iob[2]. Turns out stderr is the last RAM + * allocated by the linker for this project. We usae that to keep a shadow + * of __iob[2] for stack overflow detection and other memory corruption. + */ +void xio_set_stdin(const uint8_t dev) { stdin = &ds[dev].file; } +void xio_set_stdout(const uint8_t dev) { stdout = &ds[dev].file; } +void xio_set_stderr(const uint8_t dev) +{ + stderr = &ds[dev].file; + xio.stderr_shadow = stderr; // this is the last thing in RAM, so we use it as a memory corruption canary +} diff --git a/src/xio/xio.h b/src/xio/xio.h index b6f293b..48c60e2 100755 --- a/src/xio/xio.h +++ b/src/xio/xio.h @@ -2,13 +2,21 @@ * xio.h - Xmega IO devices - common header file * Part of TinyG project * - * Copyright (c) 2010 - 2013 Alden S. Hart Jr. + * Copyright (c) 2010 - 2014 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 . * + * 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 @@ -16,52 +24,71 @@ * 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. */ -/* XIO devices are compatible with avr-gcc stdio, so formatted printing - * is supported. To use this sub-system outside of TinyG you may need + +/* XIO devices are compatible with avr-gcc stdio, so formatted printing + * is supported. To use this sub-system outside of TinyG you may need * some defines in tinyg.h. See notes at end of this file for more details. */ /* Note: anything that includes xio.h first needs the following: - * #include // needed for FILE def'n - * #include // needed for true and false - * #include // defines prog_char, PSTR + * #include // needed for FILE def'n + * #include // needed for true and false + * #include // defines prog_char, PSTR */ /* Note: This file contains load of sub-includes near the middle - * #include "xio_file.h" - * #include "xio_usart.h" - * #include "xio_spi.h" - * #include "xio_signals.h" - * (possibly more) + * #include "xio_file.h" + * #include "xio_usart.h" + * #include "xio_spi.h" + * #include "xio_signals.h" + * (possibly more) */ -#ifndef xio_h -#define xio_h +/* + * CAVEAT EMPTOR: File under "watch your ass": + * + * - Short story: Do not call ANYTHING that can print (i.e. send chars to the TX + * buffer) from a medium or hi interrupt. This obviously includes any printf() + * function, but also exception reports, cm_soft_alarm(), cm_hard_alarm() and a + * few other functions that call stdio print functions. + * + * - Longer Story: The stdio printf() functions use character drivers provided by + * tinyg to access the low-level Xmega devices. Specifically xio_putc_usb() in xio_usb.c, + * and xio_putc_rs485() in xio_rs485.c. Since stdio does not understand non-blocking + * IO these functions must block if there is no space in the TX buffer. Blocking is + * accomplished using sleep_mode(). The IO system is the only place where sleep_mode() + * is used. Everything else in TinyG is non-blocking. Sleep is woken (exited) whenever + * any interrupt fires. So there must always be a viable interrupt source running when + * you enter a sleep or the system will hang (lock up). In the IO functions this is the + * TX interupts, which fire when space becomes available in the USART for a TX char. This + * Means you cannot call a print function at or above the level of the TX interrupts, + * which are set to medium. + */ +#ifndef XIO_H_ONCE +#define XIO_H_ONCE -/************************************************************************* - * Device configurations - *************************************************************************/ // Pre-allocated XIO devices (configured devices) // Unused devices are commented out. All this needs to line up. -enum xioDevNum_t { // TYPE: DEVICE: - XIO_DEV_USB, // USART USB device - XIO_DEV_RS485, // USART RS485 device - XIO_DEV_SPI1, // SPI SPI channel #1 - XIO_DEV_SPI2, // SPI SPI channel #2 -// XIO_DEV_SPI3, // SPI SPI channel #3 -// XIO_DEV_SPI4, // SPI SPI channel #4 - XIO_DEV_PGM, // FILE Program memory file (read only) -// XIO_DEV_SD, // FILE SD card (not implemented) - XIO_DEV_COUNT // total device count (must be last entry) +enum xioDevNum_t { // TYPE: DEVICE: + XIO_DEV_USB, // USART USB device + XIO_DEV_RS485, // USART RS485 device + XIO_DEV_SPI1, // SPI SPI channel #1 + XIO_DEV_SPI2, // SPI SPI channel #2 + XIO_DEV_PGM, // FILE Program memory file (read only) + XIO_DEV_COUNT // total device count (must be last entry) }; // If your change these ^, check these v -#define XIO_DEV_USART_COUNT 2 // # of USART devices -#define XIO_DEV_USART_OFFSET 0 // offset for computing indices +#define XIO_DEV_USART_COUNT 2 // # of USART devices +#define XIO_DEV_USART_OFFSET 0 // offset for computing indices + +#define XIO_DEV_SPI_COUNT 2 // # of SPI devices +#define XIO_DEV_SPI_OFFSET XIO_DEV_USART_COUNT // offset for computing indicies -#define XIO_DEV_SPI_COUNT 2 // # of SPI devices -#define XIO_DEV_SPI_OFFSET XIO_DEV_USART_COUNT // offset for computing indicies +#define XIO_DEV_FILE_COUNT 1 // # of FILE devices +#define XIO_DEV_FILE_OFFSET (XIO_DEV_USART_COUNT + XIO_DEV_SPI_COUNT) // index into FILES -#define XIO_DEV_FILE_COUNT 1 // # of FILE devices -#define XIO_DEV_FILE_OFFSET (XIO_DEV_USART_COUNT + XIO_DEV_SPI_COUNT) // index into FILES +// Fast accessors +#define USB ds[XIO_DEV_USB] +#define USBu us[XIO_DEV_USB - XIO_DEV_USART_OFFSET] /****************************************************************************** * Device structures @@ -71,7 +98,7 @@ enum xioDevNum_t { // TYPE: DEVICE: * field to back-reference the generic struct so getc & putc can get at it. * Lastly there's an 'x' struct which contains data specific to each dev type. * - * The generic open() function sets up the generic struct and the FILE stream. + * The generic open() function sets up the generic struct and the FILE stream. * the device opens() set up the extended struct and bind it ot the generic. ******************************************************************************/ // NOTE" "FILE *" is another way of saying "struct __file *" @@ -79,39 +106,39 @@ enum xioDevNum_t { // TYPE: DEVICE: #define flags_t uint16_t -typedef struct xioDEVICE { // common device struct (one per dev) - // references and self references - uint16_t magic_start; // memory integrity check - uint8_t dev; // self referential device number - FILE file; // stdio FILE stream structure - void *x; // extended device struct binding (static) - - // function bindings - FILE *(*x_open)(const uint8_t dev, const char *addr, const flags_t flags); - int (*x_ctrl)(struct xioDEVICE *d, const flags_t flags); // set device control flags - int (*x_gets)(struct xioDEVICE *d, char *buf, const int size);// non-blocking line reader - int (*x_getc)(FILE *); // read char (stdio compatible) - int (*x_putc)(char, FILE *); // write char (stdio compatible) - void (*x_flow)(struct xioDEVICE *d); // flow control callback function - - // device configuration flags - uint8_t flag_block; - uint8_t flag_echo; - uint8_t flag_crlf; - uint8_t flag_ignorecr; - uint8_t flag_ignorelf; - uint8_t flag_linemode; - uint8_t flag_xoff; // xon/xoff enabled - - // private working data and runtime flags - int size; // text buffer length (dynamic) - uint8_t len; // chars read so far (buf array index) - uint8_t signal; // signal value - uint8_t flag_in_line; // used as a state variable for line reads - uint8_t flag_eol; // end of line detected - uint8_t flag_eof; // end of file detected - char *buf; // text buffer binding (can be dynamic) - uint16_t magic_end; +typedef struct xioDEVICE { // common device struct (one per dev) + // references and self references + uint16_t magic_start; // memory integrity check + uint8_t dev; // self referential device number + FILE file; // stdio FILE stream structure + void *x; // extended device struct binding (static) + + // function bindings + FILE *(*x_open)(const uint8_t dev, const char *addr, const flags_t flags); + int (*x_ctrl)(struct xioDEVICE *d, const flags_t flags); // set device control flags + int (*x_gets)(struct xioDEVICE *d, char *buf, const int size);// non-blocking line reader + int (*x_getc)(FILE *); // read char (stdio compatible) + int (*x_putc)(char, FILE *); // write char (stdio compatible) + void (*x_flow)(struct xioDEVICE *d); // flow control callback function + + // device configuration flags + uint8_t flag_block; + uint8_t flag_echo; + uint8_t flag_crlf; + uint8_t flag_ignorecr; + uint8_t flag_ignorelf; + uint8_t flag_linemode; + uint8_t flag_xoff; // xon/xoff enabled + + // private working data and runtime flags + int size; // text buffer length (dynamic) + uint8_t len; // chars read so far (buf array index) + uint8_t signal; // signal value + uint8_t flag_in_line; // used as a state variable for line reads + uint8_t flag_eol; // end of line detected + uint8_t flag_eof; // end of file detected + char *buf; // text buffer binding (can be dynamic) + uint16_t magic_end; } xioDev_t; typedef FILE *(*x_open_t)(const uint8_t dev, const char *addr, const flags_t flags); @@ -121,33 +148,28 @@ typedef int (*x_getc_t)(FILE *); typedef int (*x_putc_t)(char, FILE *); typedef void (*x_flow_t)(xioDev_t *d); -/************************************************************************* - * Sub-Includes and static allocations - *************************************************************************/ // Put all sub-includes here so only xio.h is needed elsewhere #include "xio_file.h" #include "xio_usart.h" #include "xio_spi.h" -//#include "xio_signals.h" // Static structure allocations -xioDev_t ds[XIO_DEV_COUNT]; // allocate top-level dev structs -xioUsart_t us[XIO_DEV_USART_COUNT]; // USART extended IO structs -xioSpi_t spi[XIO_DEV_SPI_COUNT]; // SPI extended IO structs -xioFile_t fs[XIO_DEV_FILE_COUNT]; // FILE extended IO structs -//xioSignals_t sig; // signal flags -extern struct controllerSingleton tg; // needed by init() for default source - -/************************************************************************* - * Function Prototypes and Macros - *************************************************************************/ +xioDev_t ds[XIO_DEV_COUNT]; // allocate top-level dev structs +xioUsart_t us[XIO_DEV_USART_COUNT]; // USART extended IO structs +xioSpi_t spi[XIO_DEV_SPI_COUNT]; // SPI extended IO structs +xioFile_t fs[XIO_DEV_FILE_COUNT]; // FILE extended IO structs +extern struct controllerSingleton tg; // needed by init() for default source // Advance RX or TX head or tail. Buffers count down, so advance is a decrement. // The zero condition is the wrap that sets the index back to the top. #define advance_buffer(buf,len) { if ((--(buf)) == 0) buf = len-1;} -// public functions (virtual class) -void xio_init(void); +// public functions (virtual class) +void xio_init(); +void xio_init_assertions(); +uint8_t xio_test_assertions(); +uint8_t xio_isbusy(); + void xio_reset_working_flags(xioDev_t *d); FILE *xio_open(const uint8_t dev, const char *addr, const flags_t flags); int xio_ctrl(const uint8_t dev, const flags_t flags); @@ -159,25 +181,22 @@ int xio_set_baud(const uint8_t dev, const uint8_t baud_rate); // generic functions (private, but at virtual level) int xio_ctrl_generic(xioDev_t *d, const flags_t flags); -void xio_open_generic(uint8_t dev, x_open_t x_open, - x_ctrl_t x_ctrl, - x_gets_t x_gets, - x_getc_t x_getc, - x_putc_t x_putc, - x_flow_t x_flow); +void xio_open_generic(uint8_t dev, x_open_t x_open, + x_ctrl_t x_ctrl, + x_gets_t x_gets, + x_getc_t x_getc, + x_putc_t x_putc, + x_flow_t x_flow); -void xio_fc_null(xioDev_t *d); // NULL flow control callback -void xio_fc_usart(xioDev_t *d); // XON/XOFF flow control callback +void xio_fc_null(xioDev_t *d); // 0 flow control callback +void xio_fc_usart(xioDev_t *d); // XON/XOFF flow control callback // std devices -void xio_init_stdio(void); // set std devs & do startup prompt +void xio_init_stdio(); // set std devs & do startup prompt void xio_set_stdin(const uint8_t dev); void xio_set_stdout(const uint8_t dev); void xio_set_stderr(const uint8_t dev); -// assertions -uint8_t xio_assertions(uint8_t *value); - /************************************************************************* * SUPPORTING DEFINTIONS - SHOULD NOT NEED TO CHANGE *************************************************************************/ @@ -185,115 +204,110 @@ uint8_t xio_assertions(uint8_t *value); * xio control flag values * * if using 32 bits must cast 1 to uint32_t for bit evaluations to work correctly - * #define XIO_BLOCK ((uint32_t)1<<1) // 32 bit example. Change flags_t to uint32_t + * #define XIO_BLOCK ((uint32_t)1<<1) // 32 bit example. Change flags_t to uint32_t */ -#define XIO_BLOCK ((uint16_t)1<<0) // enable blocking reads -#define XIO_NOBLOCK ((uint16_t)1<<1) // disable blocking reads -#define XIO_XOFF ((uint16_t)1<<2) // enable XON/OFF flow control -#define XIO_NOXOFF ((uint16_t)1<<3) // disable XON/XOFF flow control -#define XIO_ECHO ((uint16_t)1<<4) // echo reads from device to stdio -#define XIO_NOECHO ((uint16_t)1<<5) // disable echo -#define XIO_CRLF ((uint16_t)1<<6) // convert to on writes -#define XIO_NOCRLF ((uint16_t)1<<7) // do not convert to on writes -#define XIO_IGNORECR ((uint16_t)1<<8) // ignore on reads -#define XIO_NOIGNORECR ((uint16_t)1<<9) // don't ignore on reads -#define XIO_IGNORELF ((uint16_t)1<<10) // ignore on reads -#define XIO_NOIGNORELF ((uint16_t)1<<11) // don't ignore on reads -#define XIO_LINEMODE ((uint16_t)1<<12) // special read handling -#define XIO_NOLINEMODE ((uint16_t)1<<13) // no special read handling +#define XIO_BLOCK ((uint16_t)1<<0) // enable blocking reads +#define XIO_NOBLOCK ((uint16_t)1<<1) // disable blocking reads +#define XIO_XOFF ((uint16_t)1<<2) // enable XON/OFF flow control +#define XIO_NOXOFF ((uint16_t)1<<3) // disable XON/XOFF flow control +#define XIO_ECHO ((uint16_t)1<<4) // echo reads from device to stdio +#define XIO_NOECHO ((uint16_t)1<<5) // disable echo +#define XIO_CRLF ((uint16_t)1<<6) // convert to on writes +#define XIO_NOCRLF ((uint16_t)1<<7) // do not convert to on writes +#define XIO_IGNORECR ((uint16_t)1<<8) // ignore on reads +#define XIO_NOIGNORECR ((uint16_t)1<<9) // don't ignore on reads +#define XIO_IGNORELF ((uint16_t)1<<10) // ignore on reads +#define XIO_NOIGNORELF ((uint16_t)1<<11) // don't ignore on reads +#define XIO_LINEMODE ((uint16_t)1<<12) // special read handling +#define XIO_NOLINEMODE ((uint16_t)1<<13) // no special read handling /* - * Generic XIO signals and error conditions. + * Generic XIO signals and error conditions. * See signals.h for application specific signal defs and routines. */ enum xioSignals { - XIO_SIG_OK, // OK - XIO_SIG_EAGAIN, // would block - XIO_SIG_EOL, // end-of-line encountered (string has data) - XIO_SIG_EOF, // end-of-file encountered (string has no data) - XIO_SIG_OVERRUN, // buffer overrun - XIO_SIG_RESET, // cancel operation immediately - XIO_SIG_FEEDHOLD, // pause operation - XIO_SIG_CYCLE_START, // start or resume operation - XIO_SIG_QUEUE_FLUSH, // flush planner queue - XIO_SIG_DELETE, // backspace or delete character (BS, DEL) - XIO_SIG_BELL, // BELL character (BEL, ^g) - XIO_SIG_BOOTLOADER // ESC character - start bootloader + XIO_SIG_OK, // OK + XIO_SIG_EAGAIN, // would block + XIO_SIG_EOL, // end-of-line encountered (string has data) + XIO_SIG_EOF, // end-of-file encountered (string has no data) + XIO_SIG_OVERRUN, // buffer overrun + XIO_SIG_RESET, // cancel operation immediately + XIO_SIG_FEEDHOLD, // pause operation + XIO_SIG_CYCLE_START, // start or resume operation + XIO_SIG_QUEUE_FLUSH, // flush planner queue + XIO_SIG_DELETE, // backspace or delete character (BS, DEL) + XIO_SIG_BELL, // BELL character (BEL, ^g) + XIO_SIG_BOOTLOADER // ESC character - start bootloader }; -/* Some useful ASCII definitions */ - -#define NUL (char)0x00 // ASCII NUL char (0) (not "NULL" which is a pointer) -#define STX (char)0x02 // ^b - STX -#define ETX (char)0x03 // ^c - ETX -#define ENQ (char)0x05 // ^e - ENQuire -#define BEL (char)0x07 // ^g - BEL -#define BS (char)0x08 // ^h - backspace -#define TAB (char)0x09 // ^i - character -#define LF (char)0x0A // ^j - line feed -#define VT (char)0x0B // ^k - kill stop -#define CR (char)0x0D // ^m - carriage return -#define XON (char)0x11 // ^q - DC1, XON, resume -#define XOFF (char)0x13 // ^s - DC3, XOFF, pause -#define SYN (char)0x16 // ^v - SYN - Used for queue flush -#define CAN (char)0x18 // ^x - Cancel, abort -#define ESC (char)0x1B // ^[ - ESC(ape) -//#define SP (char)0x20 // ' ' Space character // defined externally -#define DEL (char)0x7F // DEL(ete) - -#define Q_EMPTY (char)0xFF // signal no character - -/* Signal character mappings */ - +// Some useful ASCII definitions +#define STX (char)0x02 // ^b - STX +#define ETX (char)0x03 // ^c - ETX +#define ENQ (char)0x05 // ^e - ENQuire +#define BEL (char)0x07 // ^g - BEL +#define BS (char)0x08 // ^h - backspace +#define TAB (char)0x09 // ^i - character +#define LF (char)0x0A // ^j - line feed +#define VT (char)0x0B // ^k - kill stop +#define CR (char)0x0D // ^m - carriage return +#define XON (char)0x11 // ^q - DC1, XON, resume +#define XOFF (char)0x13 // ^s - DC3, XOFF, pause +#define SYN (char)0x16 // ^v - SYN - Used for queue flush +#define CAN (char)0x18 // ^x - Cancel, abort +#define ESC (char)0x1B // ^[ - ESC(ape) +#define DEL (char)0x7F // DEL(ete) + +#define Q_EMPTY (char)0xFF // signal no character + +// Signal character mappings #define CHAR_RESET CAN #define CHAR_FEEDHOLD (char)'!' #define CHAR_CYCLE_START (char)'~' #define CHAR_QUEUE_FLUSH (char)'%' -//#define CHAR_BOOTLOADER ESC /* XIO return codes - * These codes are the "inner nest" for the STAT_ return codes. + * These codes are the "inner nest" for the STAT_ return codes. * The first N TG codes correspond directly to these codes. * This eases using XIO by itself (without tinyg) and simplifes using - * tinyg codes with no mapping when used together. This comes at the cost + * tinyg codes with no mapping when used together. This comes at the cost * of making sure these lists are aligned. STAT_should be based on this list. */ enum xioCodes { - XIO_OK = 0, // OK - ALWAYS ZERO - XIO_ERR, // generic error return (errors start here) - XIO_EAGAIN, // function would block here (must be called again) - XIO_NOOP, // function had no-operation - XIO_COMPLETE, // operation complete - XIO_TERMINATE, // operation terminated (gracefully) - XIO_RESET, // operation reset (ungraceful) - XIO_EOL, // function returned end-of-line - XIO_EOF, // function returned end-of-file - XIO_FILE_NOT_OPEN, // file is not open - XIO_FILE_SIZE_EXCEEDED, // maximum file size exceeded - XIO_NO_SUCH_DEVICE, // illegal or unavailable device - XIO_BUFFER_EMPTY, // more of a statement of fact than an error code - XIO_BUFFER_FULL, - XIO_BUFFER_FULL_FATAL, - XIO_INITIALIZING, // system initializing, not ready for use - XIO_ERROR_16, // reserved - XIO_ERROR_17, // reserved - XIO_ERROR_18, // reserved - XIO_ERROR_19 // NOTE: XIO codes align to here + XIO_OK = 0, // OK - ALWAYS ZERO + XIO_ERR, // generic error return errors start here + XIO_EAGAIN, // function would block here (must be called again) + XIO_NOOP, // function had no-operation + XIO_COMPLETE, // operation complete + XIO_TERMINATE, // operation terminated (gracefully) + XIO_RESET, // operation reset (ungraceful) + XIO_EOL, // function returned end-of-line + XIO_EOF, // function returned end-of-file + XIO_FILE_NOT_OPEN, // file is not open + XIO_FILE_SIZE_EXCEEDED, // maximum file size exceeded + XIO_NO_SUCH_DEVICE, // illegal or unavailable device + XIO_BUFFER_EMPTY, // more of a statement of fact than an error code + XIO_BUFFER_FULL, + XIO_BUFFER_FULL_FATAL, + XIO_INITIALIZING, // system initializing, not ready for use + XIO_ERROR_16, // reserved + XIO_ERROR_17, // reserved + XIO_ERROR_18, // reserved + XIO_ERROR_19 // NOTE: XIO codes align to here }; #define XIO_ERRNO_MAX XIO_BUFFER_FULL_NON_FATAL -/* ASCII characters used by Gcode or otherwise unavailable for special use. +/* ASCII characters used by Gcode or otherwise unavailable for special use. See NIST sections 3.3.2.2, 3.3.2.3 and Appendix E for Gcode uses. See http://www.json.org/ for JSON notation - hex char name used by: + hex char name used by: ---- ---- ---------- -------------------- - 0x00 NUL null everything + 0x00 0 null everything 0x01 SOH ctrl-A 0x02 STX ctrl-B Kinen SPI protocol 0x03 ETX ctrl-C Kinen SPI protocol @@ -311,12 +325,12 @@ enum xioCodes { 0x0F SI ctrl-O 0x10 DLE ctrl-P 0x11 DC1 ctrl-Q XOFF - 0x12 DC2 ctrl-R + 0x12 DC2 ctrl-R 0x13 DC3 ctrl-S XON - 0x14 DC4 ctrl-T + 0x14 DC4 ctrl-T 0x15 NAK ctrl-U 0x16 SYN ctrl-V - 0x17 ETB ctrl-W + 0x17 ETB ctrl-W 0x18 CAN ctrl-X TinyG / grbl software reset 0x19 EM ctrl-Y 0x1A SUB ctrl-Z @@ -331,9 +345,10 @@ enum xioCodes { 0x22 " quote JSON notation 0x23 # number Gcode parameter prefix; JSON topic prefix character 0x24 $ dollar TinyG / grbl out-of-cycle settings prefix - 0x25 & ampersand universal symbol for logical AND (not used here) - 0x26 % percent Queue Flush character (trapped and removed from serial stream) - 0x27 ' single quote + 0x25 & ampersand universal symbol for logical AND (not used here) + 0x26 % percent Queue Flush character (trapped and removed from serial stream) + Also sometimes used as a file-start and file-end character in Gcode files + 0x27 ' single quote 0x28 ( open paren Gcode comments 0x29 ) close paren Gcode comments 0x2A * asterisk Gcode expressions; JSON wildcard character @@ -343,12 +358,12 @@ enum xioCodes { 0x2E . period Gcode numbers, parameters and expressions 0x2F / fwd slash Gcode expressions & block delete char 0x3A : colon JSON notation - 0x3B ; semicolon Gcode comemnt delimiter (alternate) + 0x3B ; semicolon Gcode comemnt delimiter (alternate) 0x3C < less than Gcode expressions 0x3D = equals Gcode expressions 0x3E > greaterthan Gcode expressions 0x3F ? question mk TinyG / grbl query - 0x40 @ at symbol JSON address prefix character + 0x40 @ at symbol JSON address prefix character 0x5B [ open bracketGcode expressions 0x5C \ backslash JSON notation (escape) @@ -356,20 +371,12 @@ enum xioCodes { 0x5E ^ caret Reserved for TinyG in-cycle command prefix 0x5F _ underscore - 0x60 ` grave accnt + 0x60 ` grave accnt 0x7B { open curly JSON notation 0x7C | pipe universal symbol for logical OR (not used here) 0x7D } close curly JSON notation 0x7E ~ tilde TinyG cycle start (trapped and removed from serial stream) - 0x7F DEL + 0x7F DEL */ -//#define __UNIT_TEST_XIO // include and run xio unit tests -#ifdef __UNIT_TEST_XIO -void xio_unit_tests(void); -#define XIO_UNITS xio_unit_tests(); -#else -#define XIO_UNITS -#endif // __UNIT_TEST_XIO - -#endif +#endif // end of include guard: XIO_H_ONCE diff --git a/src/xio/xio_file.c b/src/xio/xio_file.c index 4284bb0..6e9d17e 100755 --- a/src/xio/xio_file.c +++ b/src/xio/xio_file.c @@ -1,6 +1,6 @@ /* - * xio_file.c - device driver for program memory "files" - * - works with avr-gcc stdio library + * xio_file.c - device driver for program memory "files" + * - works with avr-gcc stdio library * * Part of TinyG project * @@ -19,33 +19,33 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include // precursor for xio.h -#include // true and false -#include // for memset -#include // precursor for xio.h -#include "../xio.h" // includes for all devices are in here +#include // precursor for xio.h +#include // true and false +#include // for memset +#include // precursor for xio.h +#include "xio.h" // includes for all devices are in here /****************************************************************************** * FILE CONFIGURATION RECORDS ******************************************************************************/ struct cfgFILE { - x_open_t x_open; // see xio.h for typedefs - x_ctrl_t x_ctrl; - x_gets_t x_gets; - x_getc_t x_getc; - x_putc_t x_putc; - x_flow_t x_flow; + x_open_t x_open; // see xio.h for typedefs + x_ctrl_t x_ctrl; + x_gets_t x_gets; + x_getc_t x_getc; + x_putc_t x_putc; + x_flow_t x_flow; }; static struct cfgFILE const cfgFile[] PROGMEM = { -{ // PGM config - xio_open_file, // open function - xio_ctrl_generic, // ctrl function - xio_gets_pgm, // get string function - xio_getc_pgm, // stdio getc function - xio_putc_pgm, // stdio putc function - xio_fc_null, // flow control callback +{ // PGM config + xio_open_file, // open function + xio_ctrl_generic, // ctrl function + xio_gets_pgm, // get string function + xio_getc_pgm, // stdio getc function + xio_putc_pgm, // stdio putc function + xio_fc_null, // flow control callback } }; /****************************************************************************** @@ -53,40 +53,40 @@ static struct cfgFILE const cfgFile[] PROGMEM = { ******************************************************************************/ /* - * xio_init_file() - initialize and set controls for file IO + * xio_init_file() - initialize and set controls for file IO * - * Need to bind the open function or a subsequent opens will fail + * Need to bind the open function or a subsequent opens will fail */ void xio_init_file() { - for (uint8_t i=0; ix = &fs[dev - XIO_DEV_FILE_OFFSET]; // bind extended struct to device - xioFile_t *dx = (xioFile_t *)d->x; + xioDev_t *d = (xioDev_t *)&ds[dev]; + d->x = &fs[dev - XIO_DEV_FILE_OFFSET]; // bind extended struct to device + xioFile_t *dx = (xioFile_t *)d->x; - memset (dx, 0, sizeof(xioFile_t)); // clear all values - xio_reset_working_flags(d); - xio_ctrl_generic(d, flags); // setup control flags - dx->filebase_P = (PROGMEM const char *)addr; // might want to range check this - dx->max_offset = PGM_ADDR_MAX; - return(&d->file); // return pointer to the FILE stream + memset (dx, 0, sizeof(xioFile_t)); // clear all values + xio_reset_working_flags(d); + xio_ctrl_generic(d, flags); // setup control flags + dx->filebase_P = (PROGMEM const char *)addr; // might want to range check this + dx->max_offset = PGM_ADDR_MAX; + return(&d->file); // return pointer to the FILE stream } diff --git a/src/xio/xio_file.h b/src/xio/xio_file.h index 2ea24a7..0846738 100755 --- a/src/xio/xio_file.h +++ b/src/xio/xio_file.h @@ -1,6 +1,6 @@ /* - * xio_file.h - device driver for file-type devices - * - works with avr-gcc stdio library + * xio_file.h - device driver for file-type devices + * - works with avr-gcc stdio library * * Part of TinyG project * @@ -23,47 +23,47 @@ Setup a memory file (OK, it's really just a string) should be declared as so: - const char g0_test1[] PROGMEM= "\ - g0 x10 y20 z30\n\ - g0 x0 y21 z-34.2"; + const char g0_test1[] PROGMEM= "\ + g0 x10 y20 z30\n\ + g0 x0 y21 z-34.2"; - Line 1 is the initial declaration of the array (string) in program memory - Line 2 is a continuation line. - - Must end with a newline and a continuation backslash - - (alternately can use a semicolon instead of \n is XIO_SEMICOLONS is set) - - Each line will be read as a single line of text using fgets() - Line 3 is the terminating line. Note the closing quote and semicolon. + Line 1 is the initial declaration of the array (string) in program memory + Line 2 is a continuation line. + - Must end with a newline and a continuation backslash + - (alternately can use a semicolon instead of \n is XIO_SEMICOLONS is set) + - Each line will be read as a single line of text using fgets() + Line 3 is the terminating line. Note the closing quote and semicolon. Initialize: xio_pgm_init() must be called first. See the routine for options. Open the file: xio_pgm_open() is called, like so: - xio_pgm_open(PGMFILE(&g0_test1)); // simple linear motion test + xio_pgm_open(PGMFILE(&g0_test1)); // simple linear motion test - PGMFILE does the right cast. If someone more familiar with all this - can explain why PSTR doesn't work I'd be grateful. + PGMFILE does the right cast. If someone more familiar with all this + can explain why PSTR doesn't work I'd be grateful. Read a line of text. Example from parsers.c - if (fgets(textbuf, BUF_LEN, srcin) == NULL) { - printf_P(PSTR("\r\nEnd of file encountered\r\n")); - clearerr(srcin); - srcin = stdin; - tg_prompt(); - return; - } + if (fgets(textbuf, BUF_LEN, srcin) == 0) { + printf_P(PSTR("\r\nEnd of file encountered\r\n")); + clearerr(srcin); + srcin = stdin; + tg_prompt(); + return; + } */ #ifndef xio_file_h #define xio_file_h -#define PGMFILE (const PROGMEM char *) // extends pgmspace.h +#define PGMFILE (const PROGMEM char *) // extends pgmspace.h /* * FILE DEVICE CONFIGS */ #define PGM_FLAGS (XIO_BLOCK | XIO_CRLF | XIO_LINEMODE) -#define PGM_ADDR_MAX (0x4000) // 16K +#define PGM_ADDR_MAX (0x4000) // 16K /* * FILE device extended control structure @@ -73,20 +73,20 @@ // file-type device control struct typedef struct xioFILE { - uint32_t rd_offset; // read index into file - uint32_t wr_offset; // write index into file - uint32_t max_offset; // max size of file - const char * filebase_P; // base location in program memory (PROGMEM) + uint32_t rd_offset; // read index into file + uint32_t wr_offset; // write index into file + uint32_t max_offset; // max size of file + const char * filebase_P; // base location in program memory (PROGMEM) } xioFile_t; /* * FILE DEVICE FUNCTION PROTOTYPES */ -void xio_init_file(void); +void xio_init_file(); FILE *xio_open_file(const uint8_t dev, const char *addr, const flags_t flags); -int xio_gets_pgm(xioDev_t *d, char *buf, const int size); // read string from program memory -int xio_getc_pgm(FILE *stream); // get a character from PROGMEM -int xio_putc_pgm(const char c, FILE *stream); // always returns ERROR +int xio_gets_pgm(xioDev_t *d, char *buf, const int size); // read string from program memory +int xio_getc_pgm(FILE *stream); // get a character from PROGMEM +int xio_putc_pgm(const char c, FILE *stream); // always returns ERROR // SD Card functions diff --git a/src/xio/xio_pgm.c b/src/xio/xio_pgm.c index 85790cb..aa5d15d 100755 --- a/src/xio/xio_pgm.c +++ b/src/xio/xio_pgm.c @@ -1,6 +1,6 @@ /* - * xio_pgm.c - device driver for program memory "files" - * - works with avr-gcc stdio library + * xio_pgm.c - device driver for program memory "files" + * - works with avr-gcc stdio library * * Part of TinyG project * @@ -19,34 +19,34 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include // precursor for xio.h -#include // true and false -#include // precursor for xio.h -#include "../xio.h" // includes for all devices are in here +#include // precursor for xio.h +#include // true and false +#include // precursor for xio.h +#include "xio.h" // includes for all devices are in here // Fast accessors (cheating) -#define PGM ds[XIO_DEV_PGM] // device struct accessor -#define PGMf fs[XIO_DEV_PGM - XIO_DEV_FILE_OFFSET] // file extended struct accessor +#define PGM ds[XIO_DEV_PGM] // device struct accessor +#define PGMf fs[XIO_DEV_PGM - XIO_DEV_FILE_OFFSET] // file extended struct accessor /* - * xio_gets_pgm() - main loop task for program memory device + * xio_gets_pgm() - main loop task for program memory device * - * Non-blocking, run-to-completion return a line from memory - * Note: LINEMODE flag is ignored. It's ALWAYS LINEMODE here. + * Non-blocking, run-to-completion return a line from memory + * Note: LINEMODE flag is ignored. It's ALWAYS LINEMODE here. */ int xio_gets_pgm(xioDev_t *d, char *buf, const int size) { - if ((PGMf.filebase_P) == 0) { // return error if no file is open - return (XIO_FILE_NOT_OPEN); - } - PGM.signal = XIO_SIG_OK; // initialize signal - if (fgets(buf, size, &PGM.file) == NULL) { - PGMf.filebase_P = NULL; - clearerr(&PGM.file); - return (XIO_EOF); - } - return (XIO_OK); + if ((PGMf.filebase_P) == 0) { // return error if no file is open + return XIO_FILE_NOT_OPEN; + } + PGM.signal = XIO_SIG_OK; // initialize signal + if (fgets(buf, size, &PGM.file) == 0) { + PGMf.filebase_P = 0; + clearerr(&PGM.file); + return XIO_EOF; + } + return XIO_OK; } /* @@ -55,64 +55,64 @@ int xio_gets_pgm(xioDev_t *d, char *buf, const int size) * Get next character from program memory file. * * END OF FILE (EOF) - * - the first time you encounter NUL, return ETX - * - all subsequent times rreturn NUL - * This allows the higherlevel stdio routines to return a line that - * terminates with a NUL, but reads from the end of file will return - * errors. + * - the first time you encounter 0, return ETX + * - all subsequent times rreturn 0 + * This allows the higherlevel stdio routines to return a line that + * terminates with a 0, but reads from the end of file will return + * errors. * - * - return ETX (as returning NUL is indistinguishable from an error) - * - return NUL (this is NOT EOF, wich is -1 and signifies and error) + * - return ETX (as returning 0 is indistinguishable from an error) + * - return 0 (this is NOT EOF, wich is -1 and signifies and error) * * LINEMODE and SEMICOLONS behaviors - * - consider and to be EOL chars (not just ) - * - also consider semicolons (';') to be EOL chars if SEMICOLONS enabled - * - convert any EOL char to to signal end-of-string (e.g. to fgets()) + * - consider and to be EOL chars (not just ) + * - also consider semicolons (';') to be EOL chars if SEMICOLONS enabled + * - convert any EOL char to to signal end-of-string (e.g. to fgets()) * * ECHO behaviors - * - if ECHO is enabled echo character to stdout - * - echo all line termination chars as newlines ('\n') - * - Note: putc should expand newlines to + * - if ECHO is enabled echo character to stdout + * - echo all line termination chars as newlines ('\n') + * - Note: putc should expand newlines to */ int xio_getc_pgm(FILE *stream) { - char c; + char c; - if (PGM.flag_eof ) { - PGM.signal = XIO_SIG_EOF; - return (_FDEV_EOF); - } - if ((c = pgm_read_byte(&PGMf.filebase_P[PGMf.rd_offset])) == NUL) { - PGM.flag_eof = true; - } - ++PGMf.rd_offset; + if (PGM.flag_eof ) { + PGM.signal = XIO_SIG_EOF; + return _FDEV_EOF; + } + if ((c = pgm_read_byte(&PGMf.filebase_P[PGMf.rd_offset])) == 0) { + PGM.flag_eof = true; + } + ++PGMf.rd_offset; - // processing is simple if not in LINEMODE - if (PGM.flag_linemode == false) { - if (PGM.flag_echo) putchar(c); // conditional echo - return (c); - } + // processing is simple if not in LINEMODE + if (PGM.flag_linemode == false) { + if (PGM.flag_echo) putchar(c); // conditional echo + return c; + } - // now do the LINEMODE stuff - if (c == NUL) { // perform newline substitutions - c = '\n'; - } else if (c == '\r') { - c = '\n'; - } - if (PGM.flag_echo) putchar(c); // conditional echo - return (c); + // now do the LINEMODE stuff + if (c == 0) { // perform newline substitutions + c = '\n'; + } else if (c == '\r') { + c = '\n'; + } + if (PGM.flag_echo) putchar(c); // conditional echo + return c; } /* - * xio_putc_pgm() - write character to to program memory device + * xio_putc_pgm() - write character to to program memory device * * Always returns error. You cannot write to program memory */ int xio_putc_pgm(const char c, FILE *stream) { - return -1; // always returns an error. Big surprise. + return -1; // always returns an error. Big surprise. } diff --git a/src/xio/xio_rs485.c b/src/xio/xio_rs485.c index 2ff1a75..f2637a5 100755 --- a/src/xio/xio_rs485.c +++ b/src/xio/xio_rs485.c @@ -1,6 +1,6 @@ /* - * xio_rs485.c - RS-485 device driver for xmega family - * - works with avr-gcc stdio library + * xio_rs485.c - RS-485 device driver for xmega family + * - works with avr-gcc stdio library * * Part of TinyG project * @@ -29,19 +29,19 @@ * driver deals with this constraint. */ -#include // precursor for xio.h -#include // true and false -#include // precursor for xio.h +#include // precursor for xio.h +#include // true and false +#include // precursor for xio.h #include -#include // needed for blocking character writes +#include // needed for blocking character writes -#include "../xio.h" +#include "xio.h" #include "../xmega/xmega_interrupts.h" -#include "../tinyg.h" // needed for canonical machine -#include "../hardware.h" // needed for hardware reset -#include "../controller.h" // needed for trapping kill char -#include "../canonical_machine.h" // needed for fgeedhold and cycle start +#include "../tinyg.h" // needed for canonical machine +#include "../hardware.h" // needed for hardware reset +#include "../controller.h" // needed for trapping kill char +#include "../canonical_machine.h" // needed for fgeedhold and cycle start // Fast accessors #define RS ds[XIO_DEV_RS485] @@ -49,75 +49,75 @@ /* * Helper functions - * xio_enable_rs485_tx() - specialized routine to enable rs488 TX mode - * xio_enable_rs485_rx() - specialized routine to enable rs488 RX mode + * xio_enable_rs485_tx() - specialized routine to enable rs488 TX mode + * xio_enable_rs485_rx() - specialized routine to enable rs488 RX mode * - * enables one mode and disables the other + * enables one mode and disables the other */ void xio_enable_rs485_tx() { - // enable TX, related interrupts & set DE and RE lines (disabling RX) - RSu.usart->CTRLB = USART_TXEN_bm; - RSu.usart->CTRLA = CTRLA_RXOFF_TXON_TXCON; - RSu.port->OUTSET = (RS485_DE_bm | RS485_RE_bm); + // enable TX, related interrupts & set DE and RE lines (disabling RX) + RSu.usart->CTRLB = USART_TXEN_bm; + RSu.usart->CTRLA = CTRLA_RXOFF_TXON_TXCON; + RSu.port->OUTSET = (RS485_DE_bm | RS485_RE_bm); } void xio_enable_rs485_rx() { - // enable RX, related interrupts & clr DE and RE lines (disabling TX) - RSu.usart->CTRLB = USART_RXEN_bm; - RSu.usart->CTRLA = CTRLA_RXON_TXOFF_TXCON; - RSu.port->OUTCLR = (RS485_DE_bm | RS485_RE_bm); + // enable RX, related interrupts & clr DE and RE lines (disabling TX) + RSu.usart->CTRLB = USART_RXEN_bm; + RSu.usart->CTRLA = CTRLA_RXON_TXOFF_TXCON; + RSu.port->OUTCLR = (RS485_DE_bm | RS485_RE_bm); } /* * xio_putc_rs485() - stdio compatible char writer for rs485 devices * - * The TX putc() / interrupt dilemma: TX interrupts occur when the - * USART DATA register is empty (ready for TX data) - and will keep - * firing as long as the TX buffer is completely empty (ready for TX - * data). So putc() and its ISR henchmen must disable interrupts when - * there's nothing left to write or they will just keep firing. + * The TX putc() / interrupt dilemma: TX interrupts occur when the + * USART DATA register is empty (ready for TX data) - and will keep + * firing as long as the TX buffer is completely empty (ready for TX + * data). So putc() and its ISR henchmen must disable interrupts when + * there's nothing left to write or they will just keep firing. * - * To make matters worse, for some reason if you enable the TX interrupts - * and TX DATA is ready, it won't actually generate an interrupt. Putc() - * must "prime" the first write itself. This requires a mutual exclusion - * region around the dequeue operation to make sure the ISR and main - * routines don't collide. + * To make matters worse, for some reason if you enable the TX interrupts + * and TX DATA is ready, it won't actually generate an interrupt. Putc() + * must "prime" the first write itself. This requires a mutual exclusion + * region around the dequeue operation to make sure the ISR and main + * routines don't collide. * - * Lastly, the system must detect the end of transmission (TX complete) - * to know when to revert the RS485 driver to RX mode. So there are 2 TX - * interrupt conditions and handlers, not 1 like other USART TXs. + * Lastly, the system must detect the end of transmission (TX complete) + * to know when to revert the RS485 driver to RX mode. So there are 2 TX + * interrupt conditions and handlers, not 1 like other USART TXs. * - * NOTE: Finding a buffer empty condition on the first byte of a string - * is common as the TX byte is often written by the task itself. + * NOTE: Finding a buffer empty condition on the first byte of a string + * is common as the TX byte is often written by the task itself. */ int xio_putc_rs485(const char c, FILE *stream) { - buffer_t next_tx_buf_head; - - if ((next_tx_buf_head = (RSu.tx_buf_head)-1) == 0) { // adv. head & wrap - next_tx_buf_head = TX_BUFFER_SIZE-1; // -1 avoids the off-by-one - } - while(next_tx_buf_head == RSu.tx_buf_tail) { // buf full. sleep or ret - if (RS.flag_block) { - sleep_mode(); - } else { - RS.signal = XIO_SIG_EAGAIN; - return(_FDEV_ERR); - } - }; - // enable TX mode and write data to TX buffer - xio_enable_rs485_tx(); // enable for TX - RSu.tx_buf_head = next_tx_buf_head; // accept next buffer head - RSu.tx_buf[RSu.tx_buf_head] = c; // ...write char to buffer - - if ((c == '\n') && (RS.flag_crlf)) { // detect LF & add CR - return RS.x_putc('\r', stream); // recurse - } - // force a TX interupt to attempt to send the character - RSu.usart->CTRLA = CTRLA_RXON_TXON; // doesn't work if you just |= it - return (XIO_OK); + buffer_t next_tx_buf_head; + + if ((next_tx_buf_head = (RSu.tx_buf_head)-1) == 0) { // adv. head & wrap + next_tx_buf_head = TX_BUFFER_SIZE-1; // -1 avoids the off-by-one + } + while(next_tx_buf_head == RSu.tx_buf_tail) { // buf full. sleep or ret + if (RS.flag_block) { + sleep_mode(); + } else { + RS.signal = XIO_SIG_EAGAIN; + return(_FDEV_ERR); + } + }; + // enable TX mode and write data to TX buffer + xio_enable_rs485_tx(); // enable for TX + RSu.tx_buf_head = next_tx_buf_head; // accept next buffer head + RSu.tx_buf[RSu.tx_buf_head] = c; // ...write char to buffer + + if ((c == '\n') && (RS.flag_crlf)) { // detect LF & add CR + return RS.x_putc('\r', stream); // recurse + } + // force a TX interupt to attempt to send the character + RSu.usart->CTRLA = CTRLA_RXON_TXON; // doesn't work if you just |= it + return XIO_OK; } /* @@ -125,66 +125,66 @@ int xio_putc_rs485(const char c, FILE *stream) * RS485_TXC_ISR - RS485 transmission complete (See notes in xio_putc_rs485) */ -ISR(RS485_TX_ISR_vect) //ISR(USARTC1_DRE_vect) // USARTC1 data register empty +ISR(RS485_TX_ISR_vect) //ISR(USARTC1_DRE_vect) // USARTC1 data register empty { - // NOTE: Assumes the USART is in TX mode before this interrupt is fired - if (RSu.tx_buf_head == RSu.tx_buf_tail) { // buffer empty - disable ints (NOTE) - RSu.usart->CTRLA = CTRLA_RXON_TXOFF_TXCON; // doesn't work if you just &= it - return; - } - if (--(RSu.tx_buf_tail) == 0) { // advance tail and wrap if needed - RSu.tx_buf_tail = TX_BUFFER_SIZE-1; // -1 avoids off-by-one error (OBOE) - } - RSu.usart->DATA = RSu.tx_buf[RSu.tx_buf_tail]; // write char to TX DATA reg + // NOTE: Assumes the USART is in TX mode before this interrupt is fired + if (RSu.tx_buf_head == RSu.tx_buf_tail) { // buffer empty - disable ints (NOTE) + RSu.usart->CTRLA = CTRLA_RXON_TXOFF_TXCON; // doesn't work if you just &= it + return; + } + if (--(RSu.tx_buf_tail) == 0) { // advance tail and wrap if needed + RSu.tx_buf_tail = TX_BUFFER_SIZE-1; // -1 avoids off-by-one error (OBOE) + } + RSu.usart->DATA = RSu.tx_buf[RSu.tx_buf_tail]; // write char to TX DATA reg } -ISR(RS485_TXC_ISR_vect) // ISR(USARTC1_TXC_vect) +ISR(RS485_TXC_ISR_vect) // ISR(USARTC1_TXC_vect) { - xio_enable_rs485_rx(); // revert to RX mode + xio_enable_rs485_rx(); // revert to RX mode } /* * RS485_RX_ISR - RS485 receiver interrupt (RX) */ -ISR(RS485_RX_ISR_vect) //ISR(USARTC1_RXC_vect) // serial port C0 RX isr +ISR(RS485_RX_ISR_vect) //ISR(USARTC1_RXC_vect) // serial port C0 RX isr { - char c; - - if ((RSu.usart->STATUS & USART_RX_DATA_READY_bm) != 0) { - c = RSu.usart->DATA; // can only read DATA once - } else { - return; // shouldn't ever happen; bit of a fail-safe here - } - - // trap async commands - do not insert into RX queue - if (c == CHAR_RESET) { // trap Kill character - hw_request_hard_reset(); - return; - } - if (c == CHAR_FEEDHOLD) { // trap feedhold signal - cm_request_feedhold(); - return; - } - if (c == CHAR_CYCLE_START) { // trap end_feedhold signal - cm_request_cycle_start(); - return; - } - // filter out CRs and LFs if they are to be ignored - if ((c == CR) && (RS.flag_ignorecr)) return; - if ((c == LF) && (RS.flag_ignorelf)) return; - - // normal character path - advance_buffer(RSu.rx_buf_head, RX_BUFFER_SIZE); - if (RSu.rx_buf_head != RSu.rx_buf_tail) { // write char unless buffer full - RSu.rx_buf[RSu.rx_buf_head] = c; // (= USARTC1.DATA;) - RSu.rx_buf_count++; - // flow control detection goes here - should it be necessary - return; - } - // buffer-full handling - if ((++RSu.rx_buf_head) > RX_BUFFER_SIZE -1) { // reset the head - RSu.rx_buf_count = RX_BUFFER_SIZE-1; // reset count for good measure - RSu.rx_buf_head = 1; - } + char c; + + if ((RSu.usart->STATUS & USART_RX_DATA_READY_bm) != 0) { + c = RSu.usart->DATA; // can only read DATA once + } else { + return; // shouldn't ever happen; bit of a fail-safe here + } + + // trap async commands - do not insert into RX queue + if (c == CHAR_RESET) { // trap Kill character + hw_request_hard_reset(); + return; + } + if (c == CHAR_FEEDHOLD) { // trap feedhold signal + cm_request_feedhold(); + return; + } + if (c == CHAR_CYCLE_START) { // trap end_feedhold signal + cm_request_cycle_start(); + return; + } + // filter out CRs and LFs if they are to be ignored + if ((c == CR) && (RS.flag_ignorecr)) return; + if ((c == LF) && (RS.flag_ignorelf)) return; + + // normal character path + advance_buffer(RSu.rx_buf_head, RX_BUFFER_SIZE); + if (RSu.rx_buf_head != RSu.rx_buf_tail) { // write char unless buffer full + RSu.rx_buf[RSu.rx_buf_head] = c; // (= USARTC1.DATA;) + RSu.rx_buf_count++; + // flow control detection goes here - should it be necessary + return; + } + // buffer-full handling + if ((++RSu.rx_buf_head) > RX_BUFFER_SIZE -1) { // reset the head + RSu.rx_buf_count = RX_BUFFER_SIZE-1; // reset count for good measure + RSu.rx_buf_head = 1; + } } diff --git a/src/xio/xio_spi.c b/src/xio/xio_spi.c index 989c0a3..8a22b6c 100755 --- a/src/xio/xio_spi.c +++ b/src/xio/xio_spi.c @@ -1,6 +1,6 @@ /* - * xio_spi.c - General purpose SPI device driver for xmega family - * - works with avr-gcc stdio library + * xio_spi.c - General purpose SPI device driver for xmega family + * - works with avr-gcc stdio library * * Part of TinyG project * @@ -27,53 +27,53 @@ * * More details: * - * - A "message" is a line of text. Examples of messages are requests from the - * master to a slave, responses to these requests, and asynchronous messages - * (from a slave) that are not tied to a request. + * - A "message" is a line of text. Examples of messages are requests from the + * master to a slave, responses to these requests, and asynchronous messages + * (from a slave) that are not tied to a request. * - * Messages are terminated with a newline (aka NL, LF, line-feed). The - * terminating NL is considered part of the message and should be transmitted. + * Messages are terminated with a newline (aka NL, LF, line-feed). The + * terminating NL is considered part of the message and should be transmitted. * - * If multiple NLs are transmitted each trailing NL is interpreted as a blank - * message. This is generally not good practice - so watch it. + * If multiple NLs are transmitted each trailing NL is interpreted as a blank + * message. This is generally not good practice - so watch it. * - * Carriage return (CR) is not recognized as a newline. A CR in a message is - * treated as any other non-special ASCII character. + * Carriage return CR is not recognized as a newline. A CR in a message is + * treated as any other non-special ASCII character. * - * NULs (0x00) are not transmitted in either direction (e.g. string terminations). - * Depending on the master or slave internals, it may convert NULs to NLs. + * 0s (0x00) are not transmitted in either direction (e.g. string terminations). + * Depending on the master or slave internals, it may convert 0s to NLs. * - * - A slave is always in RX state - it must always be able to receive message data (MOSI). + * - A slave is always in RX state - it must always be able to receive message data (MOSI). * - * - All SPI transmissions are initiated by the master and are 8 bits long. As the - * slave is receiving the byte on MOSI it should be returning the next character - * in its output buffer on MISO. Note that there is no inherent correlation between - * the char (or message) being received from the master and transmitted from the - * slave. It's just IO. + * - All SPI transmissions are initiated by the master and are 8 bits long. As the + * slave is receiving the byte on MOSI it should be returning the next character + * in its output buffer on MISO. Note that there is no inherent correlation between + * the char (or message) being received from the master and transmitted from the + * slave. It's just IO. * - * If the slave has no data to send it should return ETX (0x03) on MISO. This is - * useful to distinghuish between an "empty" slave and an unpopulated or non- - * responsive SPI slave - which would return NULs or possibly 0xFF. + * If the slave has no data to send it should return ETX (0x03) on MISO. This is + * useful to distinghuish between an "empty" slave and an unpopulated or non- + * responsive SPI slave - which would return 0s or possibly 0xFF. * - * - The master may poll for more message data from the slave by sending STX chars to - * the slave. The slave discards all STXs and simply returns output data on these - * transfers. Presumably the master would stop polling once it receives an ETX - * from the slave. + * - The master may poll for more message data from the slave by sending STX chars to + * the slave. The slave discards all STXs and simply returns output data on these + * transfers. Presumably the master would stop polling once it receives an ETX + * from the slave. */ /* ---- Low level SPI stuff ---- * - * Uses Mode3, MSB first. See Atmel Xmega A 8077.doc, page 231 + * Uses Mode3, MSB first. See Atmel Xmega A 8077.doc, page 231 */ -#include // precursor for xio.h -#include // true and false -#include // for memset -#include // precursor for xio.h +#include // precursor for xio.h +#include // true and false +#include // for memset +#include // precursor for xio.h #include -#include // needed for blocking TX +#include // needed for blocking TX -#include "../xio.h" // includes for all devices are in here +#include "xio.h" // includes for all devices are in here #include "../xmega/xmega_interrupts.h" -#include "../tinyg.h" // needed for AXES definition +#include "../tinyg.h" // needed for AXES definition // statics static char _read_rx_buffer(xioSpi_t *dx); @@ -88,55 +88,55 @@ static char _read_spi_char(xioSpi_t *dx); ******************************************************************************/ typedef struct cfgSPI { - x_open_t x_open; // see xio.h for typedefs - x_ctrl_t x_ctrl; - x_gets_t x_gets; - x_getc_t x_getc; - x_putc_t x_putc; - x_flow_t x_flow; - USART_t *usart; // usart binding or BIT_BANG if no usart used - PORT_t *comm_port; // port for SCK, MISO and MOSI - PORT_t *ssel_port; // port for slave select line - uint8_t ssbit; // slave select bit on ssel_port - uint8_t inbits; // bits to set as inputs - uint8_t outbits; // bits to set as outputs - uint8_t outclr; // output bits to initialize as CLRd - uint8_t outset; // output bits to initialize as SET + x_open_t x_open; // see xio.h for typedefs + x_ctrl_t x_ctrl; + x_gets_t x_gets; + x_getc_t x_getc; + x_putc_t x_putc; + x_flow_t x_flow; + USART_t *usart; // usart binding or BIT_BANG if no usart used + PORT_t *comm_port; // port for SCK, MISO and MOSI + PORT_t *ssel_port; // port for slave select line + uint8_t ssbit; // slave select bit on ssel_port + uint8_t inbits; // bits to set as inputs + uint8_t outbits; // bits to set as outputs + uint8_t outclr; // output bits to initialize as CLRd + uint8_t outset; // output bits to initialize as SET } cfgSpi_t; static cfgSpi_t const cfgSpi[] PROGMEM = { - { - xio_open_spi, // SPI #1 configs - xio_ctrl_generic, - xio_gets_spi, - xio_getc_spi, - xio_putc_spi, - xio_fc_null, - BIT_BANG, - &SPI_DATA_PORT, - &SPI_SS1_PORT, - SPI_SS1_bm, - SPI_INBITS_bm, - SPI_OUTBITS_bm, - SPI_OUTCLR_bm, - SPI_OUTSET_bm, - }, - { - xio_open_spi, // SPI #2 configs - xio_ctrl_generic, - xio_gets_spi, - xio_getc_spi, - xio_putc_spi, - xio_fc_null, - BIT_BANG, - &SPI_DATA_PORT, - &SPI_SS2_PORT, - SPI_SS2_bm, - SPI_INBITS_bm, - SPI_OUTBITS_bm, - SPI_OUTCLR_bm, - SPI_OUTSET_bm, - } + { + xio_open_spi, // SPI #1 configs + xio_ctrl_generic, + xio_gets_spi, + xio_getc_spi, + xio_putc_spi, + xio_fc_null, + BIT_BANG, + &SPI_DATA_PORT, + &SPI_SS1_PORT, + SPI_SS1_bm, + SPI_INBITS_bm, + SPI_OUTBITS_bm, + SPI_OUTCLR_bm, + SPI_OUTSET_bm, + }, + { + xio_open_spi, // SPI #2 configs + xio_ctrl_generic, + xio_gets_spi, + xio_getc_spi, + xio_putc_spi, + xio_fc_null, + BIT_BANG, + &SPI_DATA_PORT, + &SPI_SS2_PORT, + SPI_SS2_bm, + SPI_INBITS_bm, + SPI_OUTBITS_bm, + SPI_OUTCLR_bm, + SPI_OUTSET_bm, + } }; /****************************************************************************** @@ -144,131 +144,131 @@ static cfgSpi_t const cfgSpi[] PROGMEM = { ******************************************************************************/ /* - * xio_init_spi() - top-level init for SPI sub-system + * xio_init_spi() - top-level init for SPI sub-system */ -void xio_init_spi(void) +void xio_init_spi() { - for (uint8_t i=0; ix = &spi[idx]; // setup extended struct pointer - xioSpi_t *dx = (xioSpi_t *)d->x; + xioDev_t *d = &ds[dev]; // setup device struct pointer + uint8_t idx = dev - XIO_DEV_SPI_OFFSET; + d->x = &spi[idx]; // setup extended struct pointer + xioSpi_t *dx = (xioSpi_t *)d->x; - memset (dx, 0, sizeof(xioSpi_t)); - xio_reset_working_flags(d); - xio_ctrl_generic(d, flags); + memset (dx, 0, sizeof(xioSpi_t)); + xio_reset_working_flags(d); + xio_ctrl_generic(d, flags); - // setup internal RX/TX control buffers - dx->rx_buf_head = 1; - dx->rx_buf_tail = 1; - dx->tx_buf_head = 1; - dx->tx_buf_tail = 1; + // setup internal RX/TX control buffers + dx->rx_buf_head = 1; + dx->rx_buf_tail = 1; + dx->tx_buf_head = 1; + dx->tx_buf_tail = 1; - // structure and device bindings and setup - dx->usart = (USART_t *)pgm_read_word(&cfgSpi[idx].usart); - dx->data_port = (PORT_t *)pgm_read_word(&cfgSpi[idx].comm_port); - dx->ssel_port = (PORT_t *)pgm_read_word(&cfgSpi[idx].ssel_port); + // structure and device bindings and setup + dx->usart = (USART_t *)pgm_read_word(&cfgSpi[idx].usart); + dx->data_port = (PORT_t *)pgm_read_word(&cfgSpi[idx].comm_port); + dx->ssel_port = (PORT_t *)pgm_read_word(&cfgSpi[idx].ssel_port); - dx->ssbit = (uint8_t)pgm_read_byte(&cfgSpi[idx].ssbit); - dx->data_port->DIRCLR = (uint8_t)pgm_read_byte(&cfgSpi[idx].inbits); - dx->data_port->DIRSET = (uint8_t)pgm_read_byte(&cfgSpi[idx].outbits); - dx->data_port->OUTCLR = (uint8_t)pgm_read_byte(&cfgSpi[idx].outclr); - dx->data_port->OUTSET = (uint8_t)pgm_read_byte(&cfgSpi[idx].outset); - return (&d->file); // return FILE reference + dx->ssbit = (uint8_t)pgm_read_byte(&cfgSpi[idx].ssbit); + dx->data_port->DIRCLR = (uint8_t)pgm_read_byte(&cfgSpi[idx].inbits); + dx->data_port->DIRSET = (uint8_t)pgm_read_byte(&cfgSpi[idx].outbits); + dx->data_port->OUTCLR = (uint8_t)pgm_read_byte(&cfgSpi[idx].outclr); + dx->data_port->OUTSET = (uint8_t)pgm_read_byte(&cfgSpi[idx].outset); + return &d->file; // return FILE reference } /* - * xio_gets_spi() - read a complete line (message) from an SPI device + * xio_gets_spi() - read a complete line (message) from an SPI device * - * Reads from the local RX buffer until it's empty, then reads from the - * slave until the line is complete or the slave is exhausted. Retains line - * context across calls - so it can be called multiple times. Reads as many - * characters as it can until any of the following is true: + * Reads from the local RX buffer until it's empty, then reads from the + * slave until the line is complete or the slave is exhausted. Retains line + * context across calls - so it can be called multiple times. Reads as many + * characters as it can until any of the following is true: * - * - Encounters newline indicating a complete line. Terminate the buffer - * but do not write the newlinw into the buffer. Reset line flag. Return XIO_OK. + * - Encounters newline indicating a complete line. Terminate the buffer + * but do not write the newlinw into the buffer. Reset line flag. Return XIO_OK. * - * - Encounters an empty buffer and no more data in the slave. Leave in_line - * Return XIO_EAGAIN. + * - Encounters an empty buffer and no more data in the slave. Leave in_line + * Return XIO_EAGAIN. * - * - A successful read would cause output buffer overflow. Return XIO_BUFFER_FULL + * - A successful read would cause output buffer overflow. Return XIO_BUFFER_FULL * - * Note: LINEMODE flag in device struct is ignored. It's ALWAYS LINEMODE here. - * Note: CRs are not recognized as NL chars - slaves must send LF to terminate a line + * Note: LINEMODE flag in device struct is ignored. It's ALWAYS LINEMODE here. + * Note: CRs are not recognized as NL chars - slaves must send LF to terminate a line */ int xio_gets_spi(xioDev_t *d, char *buf, const int size) { - xioSpi_t *dx = (xioSpi_t *)d->x; // get SPI device struct pointer - char c_out; + xioSpi_t *dx = (xioSpi_t *)d->x; // get SPI device struct pointer + char c_out; - // first time thru initializations - if (d->flag_in_line == false) { - d->flag_in_line = true; // yes, we are busy getting a line - d->buf = buf; // bind the output buffer - d->len = 0; // zero the buffer count - d->size = size; // set the max size of the message -// d->signal = XIO_SIG_OK; // reset signal register - } - while (true) { - if (d->len >= (d->size)-1) { // size is total count - aka 'num' in fgets() - d->buf[d->size] = NUL; // string termination preserves latest char - return (XIO_BUFFER_FULL); - } - if ((c_out = _read_rx_buffer(dx)) == Q_EMPTY) { - if ((c_out = _read_spi_char(dx)) == ETX) { // get a char from slave - return (XIO_EAGAIN); - } - } - if (c_out == LF) { - d->buf[(d->len)++] = NUL; - d->flag_in_line = false; // clear in-line state (reset) - return (XIO_OK); // return for end-of-line - } - d->buf[(d->len)++] = c_out; // write character to buffer - } + // first time thru initializations + if (d->flag_in_line == false) { + d->flag_in_line = true; // yes, we are busy getting a line + d->buf = buf; // bind the output buffer + d->len = 0; // zero the buffer count + d->size = size; // set the max size of the message +// d->signal = XIO_SIG_OK; // reset signal register + } + while (true) { + if (d->len >= (d->size)-1) { // size is total count - aka 'num' in fgets() + d->buf[d->size] = 0; // string termination preserves latest char + return XIO_BUFFER_FULL; + } + if ((c_out = _read_rx_buffer(dx)) == Q_EMPTY) { + if ((c_out = _read_spi_char(dx)) == ETX) { // get a char from slave + return XIO_EAGAIN; + } + } + if (c_out == LF) { + d->buf[(d->len)++] = 0; + d->flag_in_line = false; // clear in-line state (reset) + return XIO_OK; // return for end-of-line + } + d->buf[(d->len)++] = c_out; // write character to buffer + } } /* * xio_getc_spi() - stdio compatible character RX routine * - * This function first tries to return a character from the master's RX queue - * and if that fails it tries to get the next character from the slave. + * This function first tries to return a character from the master's RX queue + * and if that fails it tries to get the next character from the slave. * - * This function is always non-blocking or it would create a deadlock as the - * bit-banger SPI transmitter is not interrupt driven + * This function is always non-blocking or it would create a deadlock as the + * bit-banger SPI transmitter is not interrupt driven * - * This function is not optimized for transfer rate, as it returns a single - * character and has no state information about the slave. gets() is much more - * efficient. + * This function is not optimized for transfer rate, as it returns a single + * character and has no state information about the slave. gets() is much more + * efficient. */ int xio_getc_spi(FILE *stream) { - xioDev_t *d = (xioDev_t *)stream->udata; // get device struct pointer - xioSpi_t *dx = (xioSpi_t *)d->x; // get SPI device struct pointer - char c; + xioDev_t *d = (xioDev_t *)stream->udata; // get device struct pointer + xioSpi_t *dx = (xioSpi_t *)d->x; // get SPI device struct pointer + char c; - if ((c = _read_rx_buffer(dx)) == Q_EMPTY) { - if ((c = _read_spi_char(dx)) == ETX) { - d->signal = XIO_SIG_EOL; - return(_FDEV_ERR); - } - } - return (c); + if ((c = _read_rx_buffer(dx)) == Q_EMPTY) { + if ((c = _read_spi_char(dx)) == ETX) { + d->signal = XIO_SIG_EOL; + return(_FDEV_ERR); + } + } + return c; } //void _xio_tx_spi_dx(xioSpi_t *dx); @@ -276,121 +276,121 @@ int xio_getc_spi(FILE *stream) /* * xio_putc_spi() - stdio compatible character TX routine * - * Putc is split in 2: xio_putc_spi() puts the char in the TX buffer, while - * xio_tx_spi() transmits a char from the TX buffer to the slave. This is not - * necessary for a pure main-loop bitbanger, but is needed for interrupts or - * other asynchronous IO processing. + * Putc is split in 2: xio_putc_spi() puts the char in the TX buffer, while + * xio_tx_spi() transmits a char from the TX buffer to the slave. This is not + * necessary for a pure main-loop bitbanger, but is needed for interrupts or + * other asynchronous IO processing. */ int xio_putc_spi(const char c, FILE *stream) { - // Native SPI device version - // write to TX queue - char TX occurs via SPI interrupt -// return ((int)_write_tx_buffer(((xioDev_t *)stream->udata)->x,c)); + // Native SPI device version + // write to TX queue - char TX occurs via SPI interrupt +// return (int_write_tx_buffer(((xioDev_t *)stream->udata)->x,c)); - // bit banger version - unbuffered IO - xioSpi_t *dx = ((xioDev_t *)stream->udata)->x; - char c_in; + // bit banger version - unbuffered IO + xioSpi_t *dx = ((xioDev_t *)stream->udata)->x; + char c_in; - if ((c_in = _xfer_spi_char(dx,c)) != ETX) { - if ((c_in == 0x00) || (c_in == 0xFF)) { - return (XIO_NO_SUCH_DEVICE); - } - _write_rx_buffer(dx, c_in); - } - return (XIO_OK); + if ((c_in = _xfer_spi_char(dx,c)) != ETX) { + if ((c_in == 0x00) || (c_in == 0xFF)) { + return XIO_NO_SUCH_DEVICE; + } + _write_rx_buffer(dx, c_in); + } + return XIO_OK; } /* - int status = _write_tx_buffer(dx,c); - if (status != XIO_OK) { return (status);} - char c_out, c_in; - if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return ();} - if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { - _write_rx_buffer(dx, c_in); - } - return (XIO_OK); + int status = _write_tx_buffer(dx,c); + if (status != XIO_OK) { return status;} + char c_out, c_in; + if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return ();} + if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { + _write_rx_buffer(dx, c_in); + } + return XIO_OK; } */ /* * xio_tx_spi() - send a character trom the TX buffer to the slave * - * Send a char to the slave while receiving a char from the slave on MISO. - * Load received char into the RX buffer if it's a legitimate.character. + * Send a char to the slave while receiving a char from the slave on MISO. + * Load received char into the RX buffer if it's a legitimate.character. */ void xio_tx_spi(uint8_t dev) { - xioDev_t *d = &ds[dev]; - xioSpi_t *dx = (xioSpi_t *)d->x; - char c_out, c_in; + xioDev_t *d = &ds[dev]; + xioSpi_t *dx = (xioSpi_t *)d->x; + char c_out, c_in; - if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return;} - if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { - _write_rx_buffer(dx, c_in); - } + if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return;} + if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { + _write_rx_buffer(dx, c_in); + } } /* void _xio_tx_spi_dx(xioSpi_t *dx) { - char c_out, c_in; - if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return;} - if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { - _write_rx_buffer(dx, c_in); - } + char c_out, c_in; + if ((c_out = _read_tx_buffer(dx)) == Q_EMPTY) { return;} + if ((c_in = _xfer_spi_char(dx, c_out)) != ETX) { + _write_rx_buffer(dx, c_in); + } } */ /* * Buffer read and write helpers * * READ: Read from the tail. Read sequence is: - * - test buffer and return Q_empty if empty - * - read char from buffer - * - advance the tail (post-advance) - * - return C with tail left pointing to next char to be read (or no data) + * - test buffer and return Q_empty if empty + * - read char from buffer + * - advance the tail (post-advance) + * - return C with tail left pointing to next char to be read (or no data) * * WRITES: Write to the head. Write sequence is: - * - advance a temporary head (pre-advance) - * - test buffer and return Q_empty if empty - * - commit head advance to structure - * - return status with head left pointing to latest char written + * - advance a temporary head (pre-advance) + * - test buffer and return Q_empty if empty + * - commit head advance to structure + * - return status with head left pointing to latest char written * - * You can make these blocking routines by calling them in an infinite - * while() waiting for something other than Q_EMPTY to be returned. + * You can make these blocking routines by calling them in an infinite + * while() waiting for something other than Q_EMPTY to be returned. */ static char _read_rx_buffer(xioSpi_t *dx) { - if (dx->rx_buf_head == dx->rx_buf_tail) { return (Q_EMPTY);} - char c = dx->rx_buf[dx->rx_buf_tail]; - if ((--(dx->rx_buf_tail)) == 0) { dx->rx_buf_tail = SPI_RX_BUFFER_SIZE-1;} - return (c); + if (dx->rx_buf_head == dx->rx_buf_tail) { return Q_EMPTY;} + char c = dx->rx_buf[dx->rx_buf_tail]; + if ((--(dx->rx_buf_tail)) == 0) { dx->rx_buf_tail = SPI_RX_BUFFER_SIZE-1;} + return c; } static char _write_rx_buffer(xioSpi_t *dx, char c) { - spibuf_t next_buf_head = dx->rx_buf_head-1; - if (next_buf_head == 0) { next_buf_head = SPI_RX_BUFFER_SIZE-1;} - if (next_buf_head == dx->rx_buf_tail) { return (Q_EMPTY);} - dx->rx_buf[next_buf_head] = c; - dx->rx_buf_head = next_buf_head; - return (XIO_OK); + spibuf_t next_buf_head = dx->rx_buf_head-1; + if (next_buf_head == 0) { next_buf_head = SPI_RX_BUFFER_SIZE-1;} + if (next_buf_head == dx->rx_buf_tail) { return Q_EMPTY;} + dx->rx_buf[next_buf_head] = c; + dx->rx_buf_head = next_buf_head; + return XIO_OK; } static char _read_tx_buffer(xioSpi_t *dx) { - if (dx->tx_buf_head == dx->tx_buf_tail) { return (Q_EMPTY);} - char c = dx->tx_buf[dx->tx_buf_tail]; - if ((--(dx->tx_buf_tail)) == 0) { dx->tx_buf_tail = SPI_TX_BUFFER_SIZE-1;} - return (c); + if (dx->tx_buf_head == dx->tx_buf_tail) { return Q_EMPTY;} + char c = dx->tx_buf[dx->tx_buf_tail]; + if ((--(dx->tx_buf_tail)) == 0) { dx->tx_buf_tail = SPI_TX_BUFFER_SIZE-1;} + return c; } /* static char _write_tx_buffer(xioSpi_t *dx, char c) { - spibuf_t next_buf_head = dx->tx_buf_head-1; - if (next_buf_head == 0) { next_buf_head = SPI_TX_BUFFER_SIZE-1;} - if (next_buf_head == dx->tx_buf_tail) { return (Q_EMPTY);} - dx->tx_buf[next_buf_head] = c; - dx->tx_buf_head = next_buf_head; - return (XIO_OK); + spibuf_t next_buf_head = dx->tx_buf_head-1; + if (next_buf_head == 0) { next_buf_head = SPI_TX_BUFFER_SIZE-1;} + if (next_buf_head == dx->tx_buf_tail) { return Q_EMPTY;} + dx->tx_buf[next_buf_head] = c; + dx->tx_buf_head = next_buf_head; + return XIO_OK; } */ /* @@ -400,52 +400,52 @@ static char _write_tx_buffer(xioSpi_t *dx, char c) */ #define xfer_bit(mask, c_out, c_in) \ - dx->data_port->OUTCLR = SPI_SCK_bm; \ - if ((c_out & mask) == 0) { dx->data_port->OUTCLR = SPI_MOSI_bm; } \ - else { dx->data_port->OUTSET = SPI_MOSI_bm; } \ - if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ - dx->data_port->OUTSET = SPI_SCK_bm; + dx->data_port->OUTCLR = SPI_SCK_bm; \ + if ((c_out & mask) == 0) { dx->data_port->OUTCLR = SPI_MOSI_bm; } \ + else { dx->data_port->OUTSET = SPI_MOSI_bm; } \ + if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ + dx->data_port->OUTSET = SPI_SCK_bm; #define read_bit_clr(mask, c_in) \ - dx->data_port->OUTCLR = SPI_SCK_bm; \ - dx->data_port->OUTCLR = SPI_MOSI_bm; \ - if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ - dx->data_port->OUTSET = SPI_SCK_bm; + dx->data_port->OUTCLR = SPI_SCK_bm; \ + dx->data_port->OUTCLR = SPI_MOSI_bm; \ + if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ + dx->data_port->OUTSET = SPI_SCK_bm; #define read_bit_set(mask, c_in) \ - dx->data_port->OUTCLR = SPI_SCK_bm; \ - dx->data_port->OUTSET = SPI_MOSI_bm; \ - if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ - dx->data_port->OUTSET = SPI_SCK_bm; + dx->data_port->OUTCLR = SPI_SCK_bm; \ + dx->data_port->OUTSET = SPI_MOSI_bm; \ + if (dx->data_port->IN & SPI_MISO_bm) c_in |= (mask); \ + dx->data_port->OUTSET = SPI_SCK_bm; static char _xfer_spi_char(xioSpi_t *dx, char c_out) { - char c_in = 0; - dx->ssel_port->OUTCLR = dx->ssbit; // drive slave select lo (active) - xfer_bit(0x80, c_out, c_in); - xfer_bit(0x40, c_out, c_in); - xfer_bit(0x20, c_out, c_in); - xfer_bit(0x10, c_out, c_in); - xfer_bit(0x08, c_out, c_in); - xfer_bit(0x04, c_out, c_in); - xfer_bit(0x02, c_out, c_in); - xfer_bit(0x01, c_out, c_in); - dx->ssel_port->OUTSET = dx->ssbit; - return (c_in); + char c_in = 0; + dx->ssel_port->OUTCLR = dx->ssbit; // drive slave select lo (active) + xfer_bit(0x80, c_out, c_in); + xfer_bit(0x40, c_out, c_in); + xfer_bit(0x20, c_out, c_in); + xfer_bit(0x10, c_out, c_in); + xfer_bit(0x08, c_out, c_in); + xfer_bit(0x04, c_out, c_in); + xfer_bit(0x02, c_out, c_in); + xfer_bit(0x01, c_out, c_in); + dx->ssel_port->OUTSET = dx->ssbit; + return c_in; } static char _read_spi_char(xioSpi_t *dx) { - char c_in = 0; - dx->ssel_port->OUTCLR = dx->ssbit; // drive slave select lo (active) - read_bit_clr(0x80, c_in); // hard coded to send STX - read_bit_clr(0x40, c_in); - read_bit_clr(0x20, c_in); - read_bit_clr(0x10, c_in); - read_bit_clr(0x08, c_in); - read_bit_clr(0x04, c_in); - read_bit_set(0x02, c_in); - read_bit_clr(0x01, c_in); - dx->ssel_port->OUTSET = dx->ssbit; - return (c_in); + char c_in = 0; + dx->ssel_port->OUTCLR = dx->ssbit; // drive slave select lo (active) + read_bit_clr(0x80, c_in); // hard coded to send STX + read_bit_clr(0x40, c_in); + read_bit_clr(0x20, c_in); + read_bit_clr(0x10, c_in); + read_bit_clr(0x08, c_in); + read_bit_clr(0x04, c_in); + read_bit_set(0x02, c_in); + read_bit_clr(0x01, c_in); + dx->ssel_port->OUTSET = dx->ssbit; + return c_in; } diff --git a/src/xio/xio_spi.h b/src/xio/xio_spi.h index 3b20b34..4142221 100755 --- a/src/xio/xio_spi.h +++ b/src/xio/xio_spi.h @@ -1,6 +1,6 @@ /* - * xio_spi.h - General purpose SPI device driver for xmega family - * - works with avr-gcc stdio library + * xio_spi.h - General purpose SPI device driver for xmega family + * - works with avr-gcc stdio library * * Part of TinyG project * @@ -27,91 +27,72 @@ ******************************************************************************/ // SPI global accessor defines -#define SPI1 ds[XIO_DEV_SPI1] // device struct accessor -#define SPI1u sp[XIO_DEV_SPI1 - XIO_DEV_SPI_OFFSET] // usart extended struct accessor +#define SPI1 ds[XIO_DEV_SPI1] // device struct accessor +#define SPI1u sp[XIO_DEV_SPI1 - XIO_DEV_SPI_OFFSET] // usart extended struct accessor -#define SPI2 ds[XIO_DEV_SPI2] // device struct accessor -#define SPI2u sp[XIO_DEV_SPI2 - XIO_DEV_SPI_OFFSET] // usart extended struct accessor +#define SPI2 ds[XIO_DEV_SPI2] // device struct accessor +#define SPI2u sp[XIO_DEV_SPI2 - XIO_DEV_SPI_OFFSET] // usart extended struct accessor // Buffer sizing -#define spibuf_t uint_fast8_t // fast, but limits SPI buffers to 255 char max +#define spibuf_t uint_fast8_t // fast, but limits SPI buffers to 255 char max #define SPI_RX_BUFFER_SIZE (spibuf_t)64 #define SPI_TX_BUFFER_SIZE (spibuf_t)64 -// Alternates for larger buffers - mostly for debugging -//#define spibuf_t uint16_t // slower, but supports larger buffers -//#define SPI_RX_BUFFER_SIZE (spibuf_t)512 -//#define SPI_TX_BUFFER_SIZE (spibuf_t)512 -//#define SPI_RX_BUFFER_SIZE (spibuf_t)1024 -//#define SPI_TX_BUFFER_SIZE (spibuf_t)1024 - - //**** SPI device configuration **** //NOTE: XIO_BLOCK / XIO_NOBLOCK affects reads only. Writes always block. (see xio.h) #define SPI_FLAGS (XIO_BLOCK | XIO_ECHO | XIO_LINEMODE) -#define BIT_BANG 0 // use this value if no USART is being used -#define SPI_USART BIT_BANG // USB usart or BIT_BANG value -#define SPI_RX_ISR_vect BIT_BANG // (RX) reception complete IRQ -#define SPI_TX_ISR_vect BIT_BANG // (TX) data register empty IRQ - -//#define SPI_USART USARTC1 // USB usart -//#define SPI_RX_ISR_vect USARTC0_RXC_vect // (RX) reception complete IRQ -//#define SPI_TX_ISR_vect USARTC0_DRE_vect // (TX) data register empty IRQ +#define BIT_BANG 0 // use this value if no USART is being used +#define SPI_USART BIT_BANG // USB usart or BIT_BANG value +#define SPI_RX_ISR_vect BIT_BANG // (RX) reception complete IRQ +#define SPI_TX_ISR_vect BIT_BANG // (TX) data register empty IRQ // The bit mappings for SCK / MISO / MOSI / SS1 map to the xmega SPI device pinouts -#define SPI_DATA_PORT PORTC // port for SPI data lines -#define SPI_SCK_bp 7 // SCK - clock bit position (pin is wired on board) -#define SPI_MISO_bp 6 // MISO - bit position (pin is wired on board) -#define SPI_MOSI_bp 5 // MOSI - bit position (pin is wired on board) -#define SPI_SS1_PORT SPI_DATA_PORT // slave select assignments -#define SPI_SS1_bp 4 // SS1 - slave select #1 +#define SPI_DATA_PORT PORTC // port for SPI data lines +#define SPI_SCK_bp 7 // SCK - clock bit position (pin is wired on board) +#define SPI_MISO_bp 6 // MISO - bit position (pin is wired on board) +#define SPI_MOSI_bp 5 // MOSI - bit position (pin is wired on board) +#define SPI_SS1_PORT SPI_DATA_PORT // slave select assignments +#define SPI_SS1_bp 4 // SS1 - slave select #1 // additional slave selects -#define SPI_SS2_PORT PORTB -#define SPI_SS2_bp 3 // SS1 - slave select #2 +#define SPI_SS2_PORT PORTB +#define SPI_SS2_bp 3 // SS1 - slave select #2 -#define SPI_MOSI_bm (1< // precursor for xio.h -#include // true and false -#include // for memset -#include // precursor for xio.h +#include // precursor for xio.h +#include // true and false +#include // for memset +#include // precursor for xio.h #include -#include // needed for blocking character writes +#include // needed for blocking character writes -#include "../xio.h" // includes for all devices are in here +#include "xio.h" // includes for all devices are in here #include "../xmega/xmega_interrupts.h" -#include "../tinyg.h" // needed for AXES definition -#include "../gpio.h" // needed for XON/XOFF LED indicator -#include "../util.h" // needed to pick up __debug defines -#include "../config.h" // needed to write back usb baud rate +#include "../tinyg.h" // needed for AXES definition +#include "../gpio.h" // needed for XON/XOFF LED indicator +#include "../util.h" // needed to pick up __debug defines +#include "../config.h" // needed to write back usb baud rate /****************************************************************************** * USART CONFIGURATION RECORDS @@ -41,52 +41,52 @@ static const uint8_t bsel[] PROGMEM = { 0, 207, 103, 51, 34, 33, 31, 27, 19, 1, static const uint8_t bscale[] PROGMEM = { 0, 0, 0, 0, 0, (-1<<4), (-2<<4), (-3<<4), (-4<<4), (1<<4), 1 }; struct cfgUSART { - x_open_t x_open; - x_ctrl_t x_ctrl; - x_gets_t x_gets; - x_getc_t x_getc; - x_putc_t x_putc; - x_flow_t x_flow; - USART_t *usart; // usart binding - PORT_t *port; // port binding - uint8_t baud; - uint8_t inbits; - uint8_t outbits; - uint8_t outclr; - uint8_t outset; + x_open_t x_open; + x_ctrl_t x_ctrl; + x_gets_t x_gets; + x_getc_t x_getc; + x_putc_t x_putc; + x_flow_t x_flow; + USART_t *usart; // usart binding + PORT_t *port; // port binding + uint8_t baud; + uint8_t inbits; + uint8_t outbits; + uint8_t outclr; + uint8_t outset; }; static struct cfgUSART const cfgUsart[] PROGMEM = { - { - xio_open_usart, // USB config record - xio_ctrl_generic, - xio_gets_usart, - xio_getc_usart, - xio_putc_usb, - xio_fc_usart, - &USB_USART, - &USB_PORT, - USB_BAUD, - USB_INBITS_bm, - USB_OUTBITS_bm, - USB_OUTCLR_bm, - USB_OUTSET_bm - }, - { - xio_open_usart, // RS485 config record - xio_ctrl_generic, - xio_gets_usart, - xio_getc_usart, - xio_putc_rs485, - xio_fc_null, - &RS485_USART, - &RS485_PORT, - RS485_BAUD, - RS485_INBITS_bm, - RS485_OUTBITS_bm, - RS485_OUTCLR_bm, - RS485_OUTSET_bm - } + { + xio_open_usart, // USB config record + xio_ctrl_generic, + xio_gets_usart, + xio_getc_usart, + xio_putc_usb, + xio_fc_usart, + &USB_USART, + &USB_PORT, + USB_BAUD, + USB_INBITS_bm, + USB_OUTBITS_bm, + USB_OUTCLR_bm, + USB_OUTSET_bm + }, + { + xio_open_usart, // RS485 config record + xio_ctrl_generic, + xio_gets_usart, + xio_getc_usart, + xio_putc_rs485, + xio_fc_null, + &RS485_USART, + &RS485_PORT, + RS485_BAUD, + RS485_INBITS_bm, + RS485_OUTBITS_bm, + RS485_OUTCLR_bm, + RS485_OUTSET_bm + } }; /****************************************************************************** @@ -96,76 +96,76 @@ static struct cfgUSART const cfgUsart[] PROGMEM = { static int _gets_helper(xioDev_t *d, xioUsart_t *dx); /* - * xio_init_usart() - general purpose USART initialization (shared) + * xio_init_usart() - general purpose USART initialization (shared) */ -void xio_init_usart(void) +void xio_init_usart() { - for (uint8_t i=0; ix = &us[idx]; // bind extended struct to device - xioUsart_t *dx = (xioUsart_t *)d->x; - - memset (dx, 0, sizeof(xioUsart_t)); // clear all values - xio_reset_working_flags(d); - xio_ctrl_generic(d, flags); // setup control flags - if (d->flag_xoff) { // initialize flow control settings - dx->fc_state_rx = FC_IN_XON; - dx->fc_state_tx = FC_IN_XON; - } - - // setup internal RX/TX control buffers - dx->rx_buf_head = 1; // can't use location 0 in circular buffer - dx->rx_buf_tail = 1; - dx->tx_buf_head = 1; - dx->tx_buf_tail = 1; - - // baud rate and USART setup (do this last) - dx->usart = (USART_t *)pgm_read_word(&cfgUsart[idx].usart); - dx->port = (PORT_t *)pgm_read_word(&cfgUsart[idx].port); - uint8_t baud = (uint8_t)pgm_read_byte(&cfgUsart[idx].baud); - if (baud == XIO_BAUD_UNSPECIFIED) { baud = XIO_BAUD_DEFAULT; } - xio_set_baud_usart(dx, baud); // usart must be bound first - dx->port->DIRCLR = (uint8_t)pgm_read_byte(&cfgUsart[idx].inbits); - dx->port->DIRSET = (uint8_t)pgm_read_byte(&cfgUsart[idx].outbits); - dx->port->OUTCLR = (uint8_t)pgm_read_byte(&cfgUsart[idx].outclr); - dx->port->OUTSET = (uint8_t)pgm_read_byte(&cfgUsart[idx].outset); - dx->usart->CTRLB = (USART_TXEN_bm | USART_RXEN_bm); // enable tx and rx - dx->usart->CTRLA = CTRLA_RXON_TXON; // enable tx and rx IRQs - - dx->port->USB_CTS_PINCTRL = PORT_OPC_TOTEM_gc | PORT_ISC_BOTHEDGES_gc; - dx->port->INTCTRL = USB_CTS_INTLVL; // see xio_usart.h for setting - dx->port->USB_CTS_INTMSK = USB_CTS_bm; - - return (&d->file); // return FILE reference - - // here's a bag for the RS485 device - if (dev == XIO_DEV_RS485) { - xio_enable_rs485_rx(); // sets RS-485 to RX mode initially - } + xioDev_t *d = &ds[dev]; // setup device struct pointer + uint8_t idx = dev - XIO_DEV_USART_OFFSET; + d->x = &us[idx]; // bind extended struct to device + xioUsart_t *dx = (xioUsart_t *)d->x; + + memset (dx, 0, sizeof(xioUsart_t)); // clear all values + xio_reset_working_flags(d); + xio_ctrl_generic(d, flags); // setup control flags + if (d->flag_xoff) { // initialize flow control settings + dx->fc_state_rx = FC_IN_XON; + dx->fc_state_tx = FC_IN_XON; + } + + // setup internal RX/TX control buffers + dx->rx_buf_head = 1; // can't use location 0 in circular buffer + dx->rx_buf_tail = 1; + dx->tx_buf_head = 1; + dx->tx_buf_tail = 1; + + // baud rate and USART setup (do this last) + dx->usart = (USART_t *)pgm_read_word(&cfgUsart[idx].usart); + dx->port = (PORT_t *)pgm_read_word(&cfgUsart[idx].port); + uint8_t baud = (uint8_t)pgm_read_byte(&cfgUsart[idx].baud); + if (baud == XIO_BAUD_UNSPECIFIED) { baud = XIO_BAUD_DEFAULT; } + xio_set_baud_usart(dx, baud); // usart must be bound first + dx->port->DIRCLR = (uint8_t)pgm_read_byte(&cfgUsart[idx].inbits); + dx->port->DIRSET = (uint8_t)pgm_read_byte(&cfgUsart[idx].outbits); + dx->port->OUTCLR = (uint8_t)pgm_read_byte(&cfgUsart[idx].outclr); + dx->port->OUTSET = (uint8_t)pgm_read_byte(&cfgUsart[idx].outset); + dx->usart->CTRLB = (USART_TXEN_bm | USART_RXEN_bm); // enable tx and rx + dx->usart->CTRLA = CTRLA_RXON_TXON; // enable tx and rx IRQs + + dx->port->USB_CTS_PINCTRL = PORT_OPC_TOTEM_gc | PORT_ISC_BOTHEDGES_gc; + dx->port->INTCTRL = USB_CTS_INTLVL; // see xio_usart.h for setting + dx->port->USB_CTS_INTMSK = USB_CTS_bm; + + return &d->file; // return FILE reference + + // here's a bag for the RS485 device + if (dev == XIO_DEV_RS485) { + xio_enable_rs485_rx(); // sets RS-485 to RX mode initially + } } void xio_set_baud_usart(xioUsart_t *dx, const uint8_t baud) { - dx->usart->BAUDCTRLA = (uint8_t)pgm_read_byte(&bsel[baud]); - dx->usart->BAUDCTRLB = (uint8_t)pgm_read_byte(&bscale[baud]); - cfg.usb_baud_rate = baud; + dx->usart->BAUDCTRLA = (uint8_t)pgm_read_byte(&bsel[baud]); + dx->usart->BAUDCTRLB = (uint8_t)pgm_read_byte(&bscale[baud]); + cfg.usb_baud_rate = baud; } /* @@ -177,214 +177,214 @@ void xio_set_baud_usart(xioUsart_t *dx, const uint8_t baud) * xio_get_tx_bufcount_usart() - returns number of chars in TX buffer * xio_get_rx_bufcount_usart() - returns number of chars in RX buffer * - * Reminder: tx/rx queues fill from top to bottom, w/0 being the wrap location + * Reminder: tx/rx queues fill from top to bottom, w/0 being the wrap location */ void xio_xoff_usart(xioUsart_t *dx) { - if (dx->fc_state_rx == FC_IN_XON) { - dx->fc_state_rx = FC_IN_XOFF; - - // If using XON/XOFF flow control - if (cfg.enable_flow_control == FLOW_CONTROL_XON) { - dx->fc_char_rx = XOFF; - dx->usart->CTRLA = CTRLA_RXON_TXON; // force a TX interrupt - } - - // If using hardware flow control. The CTS pin on the *FTDI* is our RTS. - // Logic 1 means we're NOT ready for more data. - if (cfg.enable_flow_control == FLOW_CONTROL_RTS) { - dx->port->OUTSET = USB_RTS_bm; - } - } + if (dx->fc_state_rx == FC_IN_XON) { + dx->fc_state_rx = FC_IN_XOFF; + + // If using XON/XOFF flow control + if (cfg.enable_flow_control == FLOW_CONTROL_XON) { + dx->fc_char_rx = XOFF; + dx->usart->CTRLA = CTRLA_RXON_TXON; // force a TX interrupt + } + + // If using hardware flow control. The CTS pin on the *FTDI* is our RTS. + // Logic 1 means we're NOT ready for more data. + if (cfg.enable_flow_control == FLOW_CONTROL_RTS) { + dx->port->OUTSET = USB_RTS_bm; + } + } } void xio_xon_usart(xioUsart_t *dx) { - if (dx->fc_state_rx == FC_IN_XOFF) { - dx->fc_state_rx = FC_IN_XON; - - // If using XON/XOFF flow control - if (cfg.enable_flow_control == FLOW_CONTROL_XON) { - dx->fc_char_rx = XON; - dx->usart->CTRLA = CTRLA_RXON_TXON; // force a TX interrupt - } - - // If using hardware flow control. The CTS pin on the *FTDI* is our RTS. - // Logic 0 means we're ready for more data. - if (cfg.enable_flow_control == FLOW_CONTROL_RTS) { - dx->port->OUTCLR = USB_RTS_bm; - } - } + if (dx->fc_state_rx == FC_IN_XOFF) { + dx->fc_state_rx = FC_IN_XON; + + // If using XON/XOFF flow control + if (cfg.enable_flow_control == FLOW_CONTROL_XON) { + dx->fc_char_rx = XON; + dx->usart->CTRLA = CTRLA_RXON_TXON; // force a TX interrupt + } + + // If using hardware flow control. The CTS pin on the *FTDI* is our RTS. + // Logic 0 means we're ready for more data. + if (cfg.enable_flow_control == FLOW_CONTROL_RTS) { + dx->port->OUTCLR = USB_RTS_bm; + } + } } -void xio_fc_usart(xioDev_t *d) // callback from the usart handlers +void xio_fc_usart(xioDev_t *d) // callback from the usart handlers { - xioUsart_t *dx = d->x; - if (xio_get_rx_bufcount_usart(dx) < XOFF_RX_LO_WATER_MARK) { - xio_xon_usart(dx); - } + xioUsart_t *dx = d->x; + if (xio_get_rx_bufcount_usart(dx) < XOFF_RX_LO_WATER_MARK) { + xio_xon_usart(dx); + } } buffer_t xio_get_tx_bufcount_usart(const xioUsart_t *dx) { - if (dx->tx_buf_head <= dx->tx_buf_tail) { - return (dx->tx_buf_tail - dx->tx_buf_head); - } else { - return (TX_BUFFER_SIZE - (dx->tx_buf_head - dx->tx_buf_tail)); - } + if (dx->tx_buf_head <= dx->tx_buf_tail) { + return dx->tx_buf_tail - dx->tx_buf_head; + } else { + return TX_BUFFER_SIZE - (dx->tx_buf_head - dx->tx_buf_tail); + } } buffer_t xio_get_rx_bufcount_usart(const xioUsart_t *dx) { -// return (dx->rx_buf_count); - if (dx->rx_buf_head <= dx->rx_buf_tail) { - return (dx->rx_buf_tail - dx->rx_buf_head); - } else { - return (RX_BUFFER_SIZE - (dx->rx_buf_head - dx->rx_buf_tail)); - } +// return dx->rx_buf_count; + if (dx->rx_buf_head <= dx->rx_buf_tail) { + return dx->rx_buf_tail - dx->rx_buf_head; + } else { + return RX_BUFFER_SIZE - (dx->rx_buf_head - dx->rx_buf_tail); + } } /* - * xio_gets_usart() - read a complete line from the usart device - * _gets_helper() - non-blocking character getter for gets + * xio_gets_usart() - read a complete line from the usart device + * _gets_helper() - non-blocking character getter for gets * - * Retains line context across calls - so it can be called multiple times. - * Reads as many characters as it can until any of the following is true: + * Retains line context across calls - so it can be called multiple times. + * Reads as many characters as it can until any of the following is true: * - * - RX buffer is empty on entry (return XIO_EAGAIN) - * - no more chars to read from RX buffer (return XIO_EAGAIN) - * - read would cause output buffer overflow (return XIO_BUFFER_FULL) - * - read returns complete line (returns XIO_OK) + * - RX buffer is empty on entry (return XIO_EAGAIN) + * - no more chars to read from RX buffer (return XIO_EAGAIN) + * - read would cause output buffer overflow (return XIO_BUFFER_FULL) + * - read returns complete line (returns XIO_OK) * - * Note: LINEMODE flag in device struct is ignored. It's ALWAYS LINEMODE here. - * Note: This function assumes ignore CR and ignore LF handled upstream before the RX buffer + * Note: LINEMODE flag in device struct is ignored. It's ALWAYS LINEMODE here. + * Note: This function assumes ignore CR and ignore LF handled upstream before the RX buffer */ int xio_gets_usart(xioDev_t *d, char *buf, const int size) { - xioUsart_t *dx = d->x; // USART pointer - - if (d->flag_in_line == false) { // first time thru initializations - d->flag_in_line = true; // yes, we are busy getting a line - d->len = 0; // zero buffer - d->buf = buf; - d->size = size; - d->signal = XIO_SIG_OK; // reset signal register - } - while (true) { - switch (_gets_helper(d,dx)) { - case (XIO_BUFFER_EMPTY): return (XIO_EAGAIN); // empty condition - case (XIO_BUFFER_FULL): return (XIO_BUFFER_FULL);// overrun err - case (XIO_EOL): return (XIO_OK); // got complete line - case (XIO_EAGAIN): break; // loop for next character - } - } - return (XIO_OK); + xioUsart_t *dx = d->x; // USART pointer + + if (d->flag_in_line == false) { // first time thru initializations + d->flag_in_line = true; // yes, we are busy getting a line + d->len = 0; // zero buffer + d->buf = buf; + d->size = size; + d->signal = XIO_SIG_OK; // reset signal register + } + while (true) { + switch (_gets_helper(d,dx)) { + case (XIO_BUFFER_EMPTY): return XIO_EAGAIN; // empty condition + case (XIO_BUFFER_FULL): return XIO_BUFFER_FULL;// overrun err + case (XIO_EOL): return XIO_OK; // got complete line + case (XIO_EAGAIN): break; // loop for next character + } + } + return XIO_OK; } static int _gets_helper(xioDev_t *d, xioUsart_t *dx) { - char c = NUL; - - if (dx->rx_buf_head == dx->rx_buf_tail) { // RX ISR buffer empty - dx->rx_buf_count = 0; // reset count for good measure - return(XIO_BUFFER_EMPTY); // stop reading - } - advance_buffer(dx->rx_buf_tail, RX_BUFFER_SIZE); - dx->rx_buf_count--; - d->x_flow(d); // run flow control -// c = dx->rx_buf[dx->rx_buf_tail]; // get char from RX Q - c = (dx->rx_buf[dx->rx_buf_tail] & 0x007F); // get char from RX Q & mask MSB - if (d->flag_echo) d->x_putc(c, stdout); // conditional echo regardless of character - - if (d->len >= d->size) { // handle buffer overruns - d->buf[d->size] = NUL; // terminate line (d->size is zero based) - d->signal = XIO_SIG_EOL; - return (XIO_BUFFER_FULL); - } - if ((c == CR) || (c == LF)) { // handle CR, LF termination - d->buf[(d->len)++] = NUL; - d->signal = XIO_SIG_EOL; - d->flag_in_line = false; // clear in-line state (reset) - return (XIO_EOL); // return for end-of-line - } - d->buf[(d->len)++] = c; // write character to buffer - return (XIO_EAGAIN); + char c = 0; + + if (dx->rx_buf_head == dx->rx_buf_tail) { // RX ISR buffer empty + dx->rx_buf_count = 0; // reset count for good measure + return(XIO_BUFFER_EMPTY); // stop reading + } + advance_buffer(dx->rx_buf_tail, RX_BUFFER_SIZE); + dx->rx_buf_count--; + d->x_flow(d); // run flow control +// c = dx->rx_buf[dx->rx_buf_tail]; // get char from RX Q + c = (dx->rx_buf[dx->rx_buf_tail] & 0x007F); // get char from RX Q & mask MSB + if (d->flag_echo) d->x_putc(c, stdout); // conditional echo regardless of character + + if (d->len >= d->size) { // handle buffer overruns + d->buf[d->size] = 0; // terminate line (d->size is zero based) + d->signal = XIO_SIG_EOL; + return XIO_BUFFER_FULL; + } + if ((c == CR) || (c == LF)) { // handle CR, LF termination + d->buf[(d->len)++] = 0; + d->signal = XIO_SIG_EOL; + d->flag_in_line = false; // clear in-line state (reset) + return XIO_EOL; // return for end-of-line + } + d->buf[(d->len)++] = c; // write character to buffer + return XIO_EAGAIN; } /* * xio_getc_usart() - generic char reader for USART devices * - * Compatible with stdio system - may be bound to a FILE handle + * Compatible with stdio system - may be bound to a FILE handle * * Get next character from RX buffer. * See https://www.synthetos.com/wiki/index.php?title=Projects:TinyG-Module-Details#Notes_on_Circular_Buffers * for a discussion of how the circular buffers work * - * This routine returns a single character from the RX buffer to the caller. - * It's typically called by fgets() and is useful for single-threaded IO cases. - * Cases with multiple concurrent IO streams may want to use the gets() function - * which is incompatible with the stdio system. + * This routine returns a single character from the RX buffer to the caller. + * It's typically called by fgets() and is useful for single-threaded IO cases. + * Cases with multiple concurrent IO streams may want to use the gets() function + * which is incompatible with the stdio system. * * Flags that affect behavior: * * BLOCKING behaviors - * - execute blocking or non-blocking read depending on controls - * - return character or -1 & XIO_SIG_WOULDBLOCK if non-blocking - * - return character or sleep() if blocking + * - execute blocking or non-blocking read depending on controls + * - return character or -1 & XIO_SIG_WOULDBLOCK if non-blocking + * - return character or sleep() if blocking * * ECHO behaviors - * - if ECHO is enabled echo character to stdout - * - echo all line termination chars as newlines ('\n') - * - Note: putc is responsible for expanding newlines to if needed + * - if ECHO is enabled echo character to stdout + * - echo all line termination chars as newlines ('\n') + * - Note: putc is responsible for expanding newlines to if needed */ int xio_getc_usart(FILE *stream) { - // these convenience pointers optimize faster than resolving the references each time - xioDev_t *d = (xioDev_t *)stream->udata; - xioUsart_t *dx = d->x; - char c; - - while (dx->rx_buf_head == dx->rx_buf_tail) { // RX ISR buffer empty - dx->rx_buf_count = 0; // reset count for good measure - if (d->flag_block) { - sleep_mode(); - } else { - d->signal = XIO_SIG_EAGAIN; - return(_FDEV_ERR); - } - } - advance_buffer(dx->rx_buf_tail, RX_BUFFER_SIZE); - dx->rx_buf_count--; - d->x_flow(d); // flow control callback - c = (dx->rx_buf[dx->rx_buf_tail] & 0x007F); // get char from RX buf & mask MSB - - // Triage the input character for handling. This code does not handle deletes - if (d->flag_echo) d->x_putc(c, stdout); // conditional echo regardless of character - if (c > CR) return(c); // fast cutout for majority cases - if ((c == CR) || (c == LF)) { - if (d->flag_linemode) return('\n'); - } - return(c); + // these convenience pointers optimize faster than resolving the references each time + xioDev_t *d = (xioDev_t *)stream->udata; + xioUsart_t *dx = d->x; + char c; + + while (dx->rx_buf_head == dx->rx_buf_tail) { // RX ISR buffer empty + dx->rx_buf_count = 0; // reset count for good measure + if (d->flag_block) { + sleep_mode(); + } else { + d->signal = XIO_SIG_EAGAIN; + return(_FDEV_ERR); + } + } + advance_buffer(dx->rx_buf_tail, RX_BUFFER_SIZE); + dx->rx_buf_count--; + d->x_flow(d); // flow control callback + c = (dx->rx_buf[dx->rx_buf_tail] & 0x007F); // get char from RX buf & mask MSB + + // Triage the input character for handling. This code does not handle deletes + if (d->flag_echo) d->x_putc(c, stdout); // conditional echo regardless of character + if (c > CR) return(c); // fast cutout for majority cases + if ((c == CR) || (c == LF)) { + if (d->flag_linemode) return('\n'); + } + return(c); } /* * xio_putc_usart() - stdio compatible char writer for usart devices - * This routine is not needed at the class level. - * See xio_putc_usb() and xio_putc_rs485() + * This routine is not needed at the class level. + * See xio_putc_usb() and xio_putc_rs485() */ int xio_putc_usart(const char c, FILE *stream) { - return (XIO_OK); + return XIO_OK; } /* Fakeout routines * - * xio_queue_RX_string_usart() - fake ISR to put a string in the RX buffer - * xio_queue_RX_char_usart() - fake ISR to put a char in the RX buffer + * xio_queue_RX_string_usart() - fake ISR to put a string in the RX buffer + * xio_queue_RX_char_usart() - fake ISR to put a char in the RX buffer * - * String must be NUL terminated but doesn't require a CR or LF - * Also has wrappers for USB and RS485 + * String must be 0 terminated but doesn't require a CR or LF + * Also has wrappers for USB and RS485 */ //void xio_queue_RX_char_usb(const char c) { xio_queue_RX_char_usart(XIO_DEV_USB, c); } void xio_queue_RX_string_usb(const char *buf) { xio_queue_RX_string_usart(XIO_DEV_USB, buf); } @@ -393,28 +393,28 @@ void xio_queue_RX_string_usb(const char *buf) { xio_queue_RX_string_usart(XIO_DE void xio_queue_RX_string_usart(const uint8_t dev, const char *buf) { - uint8_t i=0; - while (buf[i] != NUL) { - xio_queue_RX_char_usart(dev, buf[i++]); - } + uint8_t i=0; + while (buf[i] != 0) { + xio_queue_RX_char_usart(dev, buf[i++]); + } } void xio_queue_RX_char_usart(const uint8_t dev, const char c) { - xioDev_t *d = &ds[dev]; - xioUsart_t *dx = d->x; - - // normal path - advance_buffer(dx->rx_buf_head, RX_BUFFER_SIZE); - if (dx->rx_buf_head != dx->rx_buf_tail) { // write char unless buffer full - dx->rx_buf[dx->rx_buf_head] = c; // FAKE INPUT DATA - dx->rx_buf_count++; - return; - } - // buffer-full handling - if ((++dx->rx_buf_head) > RX_BUFFER_SIZE-1) { // reset the head - dx->rx_buf_count = RX_BUFFER_SIZE-1; - dx->rx_buf_head = 1; - } + xioDev_t *d = &ds[dev]; + xioUsart_t *dx = d->x; + + // normal path + advance_buffer(dx->rx_buf_head, RX_BUFFER_SIZE); + if (dx->rx_buf_head != dx->rx_buf_tail) { // write char unless buffer full + dx->rx_buf[dx->rx_buf_head] = c; // FAKE INPUT DATA + dx->rx_buf_count++; + return; + } + // buffer-full handling + if ((++dx->rx_buf_head) > RX_BUFFER_SIZE-1) { // reset the head + dx->rx_buf_count = RX_BUFFER_SIZE-1; + dx->rx_buf_head = 1; + } } diff --git a/src/xio/xio_usart.h b/src/xio/xio_usart.h index 1c5f0b3..bdab410 100755 --- a/src/xio/xio_usart.h +++ b/src/xio/xio_usart.h @@ -37,15 +37,6 @@ #define CTRLA_RXOFF_TXON_TXCON (USART_DREINTLVL_MED_gc | USART_TXCINTLVL_MED_gc) #define CTRLA_RXOFF_TXOFF_TXCON (USART_TXCINTLVL_MED_gc) -// Maps RX to medium and TX to lo interrupt levels -// But don't use this or exception reports and other prints from medium interrupts -// can cause the system to lock up if the TX buffer is full. See xio.h for explanation. -//#define CTRLA_RXON_TXON (USART_RXCINTLVL_MED_gc | USART_DREINTLVL_LO_gc) -//#define CTRLA_RXON_TXOFF (USART_RXCINTLVL_MED_gc) -//#define CTRLA_RXON_TXOFF_TXCON (USART_RXCINTLVL_MED_gc | USART_TXCINTLVL_LO_gc) -//#define CTRLA_RXOFF_TXON_TXCON (USART_DREINTLVL_LO_gc | USART_TXCINTLVL_LO_gc) -//#define CTRLA_RXOFF_TXOFF_TXCON (USART_TXCINTLVL_LO_gc) - // Buffer sizing #define buffer_t uint_fast8_t // fast, but limits buffer to 255 char max //#define buffer_t uint16_t // larger buffers @@ -53,15 +44,6 @@ // Must reserve 2 bytes for buffer management #define RX_BUFFER_SIZE (buffer_t)254 // buffer_t can be 8 bits #define TX_BUFFER_SIZE (buffer_t)254 // buffer_t can be 8 bits -//#define RX_BUFFER_SIZE (buffer_t)255 // buffer_t can be 8 bits -//#define TX_BUFFER_SIZE (buffer_t)255 // buffer_t can be 8 bits - -// Alternates for larger buffers - mostly for debugging -//#define buffer_t uint16_t // slower, but larger buffers -//#define RX_BUFFER_SIZE (buffer_t)510 // buffer_t must be 16 bits if >255 -//#define TX_BUFFER_SIZE (buffer_t)510 // buffer_t must be 16 bits if >255 -//#define RX_BUFFER_SIZE (buffer_t)1022 // 2046 is the practical upper limit -//#define TX_BUFFER_SIZE (buffer_t)1022 // 2046 is practical upper limit given RAM // XON/XOFF hi and lo watermarks. At 115.200 the host has approx. 100 uSec per char // to react to an XOFF. 90% (0.9) of 255 chars gives 25 chars to react, or about 2.5 ms. @@ -155,9 +137,6 @@ enum xioFCState { FC_IN_XOFF // flow controlled state }; -/****************************************************************************** - * STRUCTURES - ******************************************************************************/ /* * USART extended control structure * Note: As defined this struct won't do buffers larger than 256 chars - @@ -183,10 +162,6 @@ typedef struct xioUSART { volatile char tx_buf[TX_BUFFER_SIZE]; } xioUsart_t; -/****************************************************************************** - * USART CLASS AND DEVICE FUNCTION PROTOTYPES AND ALIASES - ******************************************************************************/ - void xio_init_usart(void); FILE *xio_open_usart(const uint8_t dev, const char *addr, const flags_t flags); void xio_set_baud_usart(xioUsart_t *dx, const uint8_t baud); diff --git a/src/xio/xio_usb.c b/src/xio/xio_usb.c index 8e15dcf..0fcf380 100755 --- a/src/xio/xio_usb.c +++ b/src/xio/xio_usb.c @@ -1,6 +1,6 @@ /* * xio_usb.c - FTDI USB device driver for xmega family - * - works with avr-gcc stdio library + * - works with avr-gcc stdio library * * Part of TinyG project * @@ -19,91 +19,91 @@ * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include // precursor for xio.h -#include // true and false -#include // precursor for xio.h +#include // precursor for xio.h +#include // true and false +#include // precursor for xio.h #include -#include // needed for blocking TX +#include // needed for blocking TX -#include "../xio.h" // includes for all devices are in here +#include "xio.h" // includes for all devices are in here #include "../xmega/xmega_interrupts.h" // application specific stuff that's littered into the USB handler #include "../tinyg.h" -#include "../config.h" // needed to find flow control setting +#include "../config.h" // needed to find flow control setting #include "../network.h" #include "../hardware.h" #include "../controller.h" -#include "../canonical_machine.h" // trapped characters communicate directly with the canonical machine +#include "../canonical_machine.h" // trapped characters communicate directly with the canonical machine /* * xio_putc_usb() * USB_TX_ISR - USB transmitter interrupt (TX) used by xio_usb_putc() * - * These are co-routines that work in tandem. - * xio_putc_usb() is a more efficient form derived from xio_putc_usart() + * These are co-routines that work in tandem. + * xio_putc_usb() is a more efficient form derived from xio_putc_usart() * - * The TX interrupt dilemma: TX interrupts occur when the USART DATA register is - * empty (and the ISR must disable interrupts when nothing's left to read, or they - * keep firing). If the TX buffer is completely empty (TXCIF is set) then enabling - * interrupts does no good. The USART won't interrupt and the TX circular buffer - * never empties. So the routine that puts chars in the TX buffer must always force - * an interrupt. + * The TX interrupt dilemma: TX interrupts occur when the USART DATA register is + * empty (and the ISR must disable interrupts when nothing's left to read, or they + * keep firing). If the TX buffer is completely empty (TXCIF is set) then enabling + * interrupts does no good. The USART won't interrupt and the TX circular buffer + * never empties. So the routine that puts chars in the TX buffer must always force + * an interrupt. */ int xio_putc_usb(const char c, FILE *stream) { - buffer_t next_tx_buf_head = USBu.tx_buf_head-1; // set next head while leaving current one alone - if (next_tx_buf_head == 0) - next_tx_buf_head = TX_BUFFER_SIZE-1; // detect wrap and adjust; -1 avoids off-by-one - while (next_tx_buf_head == USBu.tx_buf_tail) - sleep_mode(); // sleep until there is space in the buffer - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // disable TX interrupt (mutex region) - USBu.tx_buf_head = next_tx_buf_head; // accept next buffer head - USBu.tx_buf[USBu.tx_buf_head] = c; // write char to buffer - - // expand to if $ec is set - if ((c == '\n') && (USB.flag_crlf)) { - USBu.usart->CTRLA = CTRLA_RXON_TXON; // force interrupt to send the queued - buffer_t next_tx_buf_head = USBu.tx_buf_head-1; - if (next_tx_buf_head == 0) next_tx_buf_head = TX_BUFFER_SIZE-1; - while (next_tx_buf_head == USBu.tx_buf_tail) sleep_mode(); - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // MUTEX region - USBu.tx_buf_head = next_tx_buf_head; - USBu.tx_buf[USBu.tx_buf_head] = CR; - } - // finish up - USBu.usart->CTRLA = CTRLA_RXON_TXON; // force interrupt to send char(s) - doesn't work if you just |= it - return (XIO_OK); + buffer_t next_tx_buf_head = USBu.tx_buf_head-1; // set next head while leaving current one alone + if (next_tx_buf_head == 0) + next_tx_buf_head = TX_BUFFER_SIZE-1; // detect wrap and adjust; -1 avoids off-by-one + while (next_tx_buf_head == USBu.tx_buf_tail) + sleep_mode(); // sleep until there is space in the buffer + USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // disable TX interrupt (mutex region) + USBu.tx_buf_head = next_tx_buf_head; // accept next buffer head + USBu.tx_buf[USBu.tx_buf_head] = c; // write char to buffer + + // expand to if $ec is set + if ((c == '\n') && (USB.flag_crlf)) { + USBu.usart->CTRLA = CTRLA_RXON_TXON; // force interrupt to send the queued + buffer_t next_tx_buf_head = USBu.tx_buf_head-1; + if (next_tx_buf_head == 0) next_tx_buf_head = TX_BUFFER_SIZE-1; + while (next_tx_buf_head == USBu.tx_buf_tail) sleep_mode(); + USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // MUTEX region + USBu.tx_buf_head = next_tx_buf_head; + USBu.tx_buf[USBu.tx_buf_head] = CR; + } + // finish up + USBu.usart->CTRLA = CTRLA_RXON_TXON; // force interrupt to send char(s) - doesn't work if you just |= it + return (XIO_OK); } -ISR(USB_TX_ISR_vect) //ISR(USARTC0_DRE_vect) // USARTC0 data register empty +ISR(USB_TX_ISR_vect) //ISR(USARTC0_DRE_vect) // USARTC0 data register empty { - // If the CTS pin (FTDI's RTS) is HIGH, then we cannot send anything, so exit - if ((cfg.enable_flow_control == FLOW_CONTROL_RTS) && (USBu.port->IN & USB_CTS_bm)) { - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // force another TX interrupt - return; - } - - // Send an RX-side XON or XOFF character if queued - if (USBu.fc_char_rx != NUL) { // an XON/ of XOFF needs to be sent - USBu.usart->DATA = USBu.fc_char_rx; // send the XON/XOFF char and exit - USBu.fc_char_rx = NUL; - return; - } - - // Halt transmission while in TX-side XOFF - if (USBu.fc_state_tx == FC_IN_XOFF) { - return; - } - - // Otherwise process normal TX transmission - if (USBu.tx_buf_head != USBu.tx_buf_tail) { // buffer has data - advance_buffer(USBu.tx_buf_tail, TX_BUFFER_SIZE); - USBu.usart->DATA = USBu.tx_buf[USBu.tx_buf_tail]; - } else { - USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // buffer has no data; force another interrupt - } + // If the CTS pin (FTDI's RTS) is HIGH, then we cannot send anything, so exit + if ((cfg.enable_flow_control == FLOW_CONTROL_RTS) && (USBu.port->IN & USB_CTS_bm)) { + USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // force another TX interrupt + return; + } + + // Send an RX-side XON or XOFF character if queued + if (USBu.fc_char_rx != 0) { // an XON/ of XOFF needs to be sent + USBu.usart->DATA = USBu.fc_char_rx; // send the XON/XOFF char and exit + USBu.fc_char_rx = 0; + return; + } + + // Halt transmission while in TX-side XOFF + if (USBu.fc_state_tx == FC_IN_XOFF) { + return; + } + + // Otherwise process normal TX transmission + if (USBu.tx_buf_head != USBu.tx_buf_tail) { // buffer has data + advance_buffer(USBu.tx_buf_tail, TX_BUFFER_SIZE); + USBu.usart->DATA = USBu.tx_buf[USBu.tx_buf_tail]; + } else { + USBu.usart->CTRLA = CTRLA_RXON_TXOFF; // buffer has no data; force another interrupt + } } /* @@ -112,93 +112,93 @@ ISR(USB_TX_ISR_vect) //ISR(USARTC0_DRE_vect) // USARTC0 data register empty ISR(USB_CTS_ISR_vect) { - USBu.usart->CTRLA = CTRLA_RXON_TXON; // force another interrupt + USBu.usart->CTRLA = CTRLA_RXON_TXON; // force another interrupt } /* * USB_RX_ISR - USB receiver interrupt (RX) * * RX buffer states can be one of: - * - buffer has space (CTS should be asserted) - * - buffer is full (CTS should be not_asserted) - * - buffer becomes full with this character (write char and assert CTS) + * - buffer has space (CTS should be asserted) + * - buffer is full (CTS should be not_asserted) + * - buffer becomes full with this character (write char and assert CTS) * * Signals: - * - Signals are captured at the ISR level and either dispatched or flag-set - * - As RX ISR is a critical code region signal handling is stupid and fast - * - signal characters are not put in the RX buffer + * - Signals are captured at the ISR level and either dispatched or flag-set + * - As RX ISR is a critical code region signal handling is stupid and fast + * - signal characters are not put in the RX buffer * * Flow Control: - * - Flow control is not implemented. Need to work RTS line. - * - Flow control should cut off at high water mark, re-enable at low water mark - * - High water mark should have about 4 - 8 bytes left in buffer (~95% full) - * - Low water mark about 50% full + * - Flow control is not implemented. Need to work RTS line. + * - Flow control should cut off at high water mark, re-enable at low water mark + * - High water mark should have about 4 - 8 bytes left in buffer (~95% full) + * - Low water mark about 50% full */ -ISR(USB_RX_ISR_vect) //ISR(USARTC0_RXC_vect) // serial port C0 RX int +ISR(USB_RX_ISR_vect) //ISR(USARTC0_RXC_vect) // serial port C0 RX int { - char c = USBu.usart->DATA; // can only read DATA once - - if (cs.network_mode == NETWORK_MASTER) { // forward character if you are a master - net_forward(c); - } - // trap async commands - do not insert character into RX queue - if (c == CHAR_RESET) { // trap Kill signal - hw_request_hard_reset(); - return; - } - if (c == CHAR_FEEDHOLD) { // trap feedhold signal - cm_request_feedhold(); - return; - } - if (c == CHAR_QUEUE_FLUSH) { // trap queue flush signal - cm_request_queue_flush(); - return; - } - if (c == CHAR_CYCLE_START) { // trap cycle start signal - cm_request_cycle_start(); - return; - } - if (USB.flag_xoff) { - if (c == XOFF) { // trap incoming XON/XOFF signals - USBu.fc_state_tx = FC_IN_XOFF; - return; - } - if (c == XON) { - USBu.fc_state_tx = FC_IN_XON; - USBu.usart->CTRLA = CTRLA_RXON_TXOFF;// force a TX interrupt - return; - } - } - - // filter out CRs and LFs if they are to be ignored -// if ((c == CR) && (USB.flag_ignorecr)) return; // REMOVED IGNORE_CR and IGNORE LF handling -// if ((c == LF) && (USB.flag_ignorelf)) return; - - // normal character path - advance_buffer(USBu.rx_buf_head, RX_BUFFER_SIZE); - if (USBu.rx_buf_head != USBu.rx_buf_tail) { // buffer is not full - USBu.rx_buf[USBu.rx_buf_head] = c; // write char unless full - USBu.rx_buf_count++; - if ((USB.flag_xoff) && (xio_get_rx_bufcount_usart(&USBu) > XOFF_RX_HI_WATER_MARK)) { - xio_xoff_usart(&USBu); - } - } else { // buffer-full - toss the incoming character - if ((++USBu.rx_buf_head) > RX_BUFFER_SIZE-1) { // reset the head - USBu.rx_buf_count = RX_BUFFER_SIZE-1; // reset count for good measure - USBu.rx_buf_head = 1; - } - } + char c = USBu.usart->DATA; // can only read DATA once + + if (cs.network_mode == NETWORK_MASTER) { // forward character if you are a master + net_forward(c); + } + // trap async commands - do not insert character into RX queue + if (c == CHAR_RESET) { // trap Kill signal + hw_request_hard_reset(); + return; + } + if (c == CHAR_FEEDHOLD) { // trap feedhold signal + cm_request_feedhold(); + return; + } + if (c == CHAR_QUEUE_FLUSH) { // trap queue flush signal + cm_request_queue_flush(); + return; + } + if (c == CHAR_CYCLE_START) { // trap cycle start signal + cm_request_cycle_start(); + return; + } + if (USB.flag_xoff) { + if (c == XOFF) { // trap incoming XON/XOFF signals + USBu.fc_state_tx = FC_IN_XOFF; + return; + } + if (c == XON) { + USBu.fc_state_tx = FC_IN_XON; + USBu.usart->CTRLA = CTRLA_RXON_TXOFF;// force a TX interrupt + return; + } + } + + // filter out CRs and LFs if they are to be ignored +// if ((c == CR) && (USB.flag_ignorecr)) return; // REMOVED IGNORE_CR and IGNORE LF handling +// if ((c == LF) && (USB.flag_ignorelf)) return; + + // normal character path + advance_buffer(USBu.rx_buf_head, RX_BUFFER_SIZE); + if (USBu.rx_buf_head != USBu.rx_buf_tail) { // buffer is not full + USBu.rx_buf[USBu.rx_buf_head] = c; // write char unless full + USBu.rx_buf_count++; + if ((USB.flag_xoff) && (xio_get_rx_bufcount_usart(&USBu) > XOFF_RX_HI_WATER_MARK)) { + xio_xoff_usart(&USBu); + } + } else { // buffer-full - toss the incoming character + if ((++USBu.rx_buf_head) > RX_BUFFER_SIZE-1) { // reset the head + USBu.rx_buf_count = RX_BUFFER_SIZE-1; // reset count for good measure + USBu.rx_buf_head = 1; + } + } } /* * xio_get_usb_rx_free() - returns free space in the USB RX buffer * - * Remember: The queues fill from top to bottom, w/0 being the wrap location + * Remember: The queues fill from top to bottom, w/0 being the wrap location */ buffer_t xio_get_usb_rx_free(void) { - return (RX_BUFFER_SIZE - xio_get_rx_bufcount_usart(&USBu)); + return (RX_BUFFER_SIZE - xio_get_rx_bufcount_usart(&USBu)); } /* @@ -206,11 +206,11 @@ buffer_t xio_get_usb_rx_free(void) */ void xio_reset_usb_rx_buffers(void) { - // reset xio_gets() buffer - USB.len = 0; - USB.flag_in_line = false; + // reset xio_gets() buffer + USB.len = 0; + USB.flag_in_line = false; - // reset RX interrupt circular buffer - USBu.rx_buf_head = 1; // can't use location 0 in circular buffer - USBu.rx_buf_tail = 1; + // reset RX interrupt circular buffer + USBu.rx_buf_head = 1; // can't use location 0 in circular buffer + USBu.rx_buf_tail = 1; } diff --git a/src/xmega/xmega_eeprom.c b/src/xmega/xmega_eeprom.c index 25c4997..726fc60 100755 --- a/src/xmega/xmega_eeprom.c +++ b/src/xmega/xmega_eeprom.c @@ -10,13 +10,13 @@ * This file contains workarounds to those problems. * * Some code was incorporated from the avr-xboot project: - * https://github.com/alexforencich/xboot - * http://code.google.com/p/avr-xboot/wiki/Documentation + * https://github.com/alexforencich/xboot + * http://code.google.com/p/avr-xboot/wiki/Documentation * * These refs were also helpful: - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=669385 - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&t=88416 - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=89810&start=0 + * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=669385 + * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&t=88416 + * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=printview&t=89810&start=0 */ /*************************************************************************** @@ -24,21 +24,21 @@ * XMEGA EEPROM driver source file. * * This file contains the function prototypes and enumerator - * definitions for various configuration parameters for the - * XMEGA EEPROM driver. + * definitions for various configuration parameters for the + * XMEGA EEPROM driver. * * The driver is not intended for size and/or speed critical code, - * since most functions are just a few lines of code, and the - * function call overhead would decrease code performance. The driver - * is intended for rapid prototyping and documentation purposes for - * getting started with the XMEGA EEPROM module. + * since most functions are just a few lines of code, and the + * function call overhead would decrease code performance. The driver + * is intended for rapid prototyping and documentation purposes for + * getting started with the XMEGA EEPROM module. * * For size and/or speed critical code, it is recommended to copy the * function contents directly into your application instead of making * a function call (or just inline the functions, bonehead). * - * Besides which, it doesn't work in the simulator, so how would - * you ever know? + * Besides which, it doesn't work in the simulator, so how would + * you ever know? * * Notes: * See AVR1315: Accessing the XMEGA EEPROM + Code eeprom_driver.c /.h @@ -46,7 +46,7 @@ * Author: * \author * Original Author: Atmel Corporation: http://www.atmel.com - * Adapted by: Alden S. Hart Jr; 2010 + * Adapted by: Alden S. Hart Jr; 2010 * * Copyright (c) 2008, Atmel Corporation All rights reserved. * @@ -87,32 +87,32 @@ #include #include #include -#include "../tinyg.h" // only used to define __UNIT_TEST_EEPROM. can be removed. +#include "../tinyg.h" // only used to define __UNIT_TEST_EEPROM. can be removed. #ifdef __UNIT_TEST_EEPROM #include -#include // precursor for xio.h -#include // for memset() +#include // precursor for xio.h +#include // for memset() #include -#include "xio.h" // all device includes are nested here +#include "xio.h" // all device includes are nested here #endif -#define __USE_AVR1008_EEPROM // use the AVR1008 workaround code -#define ARBITRARY_MAX_LENGTH 80 // string max for NNVM write +#define __USE_AVR1008_EEPROM // use the AVR1008 workaround code +#define ARBITRARY_MAX_LENGTH 80 // string max for NNVM write /**** Inline assembly to support NVM operations ****/ -static inline void NVM_EXEC(void) +static inline void NVM_EXEC() { - void *z = (void *)&NVM_CTRLA; - - __asm__ volatile("out %[ccp], %[ioreg]" "\n\t" - "st z, %[cmdex]" - : - : [ccp] "I" (_SFR_IO_ADDR(CCP)), - [ioreg] "d" (CCP_IOREG_gc), - [cmdex] "r" (NVM_CMDEX_bm), - [z] "z" (z) + void *z = (void *)&NVM_CTRLA; + + __asm__ volatile("out %[ccp], %[ioreg]" "\n\t" + "st z, %[cmdex]" + : + : [ccp] "I" (_SFR_IO_ADDR(CCP)), + [ioreg] "d" (CCP_IOREG_gc), + [cmdex] "r" (NVM_CMDEX_bm), + [z] "z" (z) ); } @@ -124,32 +124,32 @@ static inline void NVM_EXEC(void) ISR(NVM_EE_vect) { - NVM.INTCTRL = (NVM.INTCTRL & ~NVM_EELVL_gm); // Disable EEPROM interrupt + NVM.INTCTRL = (NVM.INTCTRL & ~NVM_EELVL_gm); // Disable EEPROM interrupt } // Wrapper for NVM_EXEC that executes the workaround code -static inline void NVM_EXEC_WRAPPER(void) +static inline void NVM_EXEC_WRAPPER() { - uint8_t sleepCtr = SLEEP.CTRL; // Save the Sleep register - // Set sleep mode to IDLE - SLEEP.CTRL = (SLEEP.CTRL & ~SLEEP.CTRL) | SLEEP_SMODE_IDLE_gc; - uint8_t statusStore = PMIC.STATUS; // Save the PMIC Status... - uint8_t pmicStore = PMIC.CTRL; //...and control registers - // Enable only hi interrupts - PMIC.CTRL = (PMIC.CTRL & ~(PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm)) | PMIC_HILVLEN_bm; - uint8_t globalInt = SREG; // Save SREG for later use - sei(); // Enable global interrupts - SLEEP.CTRL |= SLEEP_SEN_bm; // Set sleep enabled - uint8_t eepromintStore = NVM.INTCTRL; // Save eeprom int settings - NVM_EXEC(); // exec EEPROM command - NVM.INTCTRL = NVM_EELVL0_bm | NVM_EELVL1_bm;// Enable EEPROM int - sleep_cpu(); // Sleep before 2.5uS passed - SLEEP.CTRL = sleepCtr; // Restore sleep settings - PMIC.STATUS = statusStore; // Restore PMIC status... - PMIC.CTRL = pmicStore; //...and control registers - NVM.INTCTRL = eepromintStore; // Restore EEPROM int settings - SREG = globalInt; // Restore global int settings + uint8_t sleepCtr = SLEEP.CTRL; // Save the Sleep register + // Set sleep mode to IDLE + SLEEP.CTRL = (SLEEP.CTRL & ~SLEEP.CTRL) | SLEEP_SMODE_IDLE_gc; + uint8_t statusStore = PMIC.STATUS; // Save the PMIC Status... + uint8_t pmicStore = PMIC.CTRL; //...and control registers + // Enable only hi interrupts + PMIC.CTRL = (PMIC.CTRL & ~(PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm)) | PMIC_HILVLEN_bm; + uint8_t globalInt = SREG; // Save SREG for later use + sei(); // Enable global interrupts + SLEEP.CTRL |= SLEEP_SEN_bm; // Set sleep enabled + uint8_t eepromintStore = NVM.INTCTRL; // Save eeprom int settings + NVM_EXEC(); // exec EEPROM command + NVM.INTCTRL = NVM_EELVL0_bm | NVM_EELVL1_bm;// Enable EEPROM int + sleep_cpu(); // Sleep before 2.5uS passed + SLEEP.CTRL = sleepCtr; // Restore sleep settings + PMIC.STATUS = statusStore; // Restore PMIC status... + PMIC.CTRL = pmicStore; //...and control registers + NVM.INTCTRL = eepromintStore; // Restore EEPROM int settings + SREG = globalInt; // Restore global int settings } #else #define NVM_EXEC_WRAPPER NVM_EXEC @@ -175,46 +175,46 @@ void NNVM_ReadBytes(const uint16_t address, int8_t *buf, const uint16_t size); void NNVM_WriteString(const uint16_t address, const char *buf, const uint8_t unused) { - uint16_t j = address; // NNVM pointer - - for (uint16_t i = 0; i < ARBITRARY_MAX_LENGTH; i++) { - nnvm[j++] = buf[i]; - if (!buf[i]) { - return; - } - } + uint16_t j = address; // NNVM pointer + + for (uint16_t i = 0; i < ARBITRARY_MAX_LENGTH; i++) { + nnvm[j++] = buf[i]; + if (!buf[i]) { + return; + } + } } void NNVM_ReadString(const uint16_t address, char *buf, const uint8_t size) { - uint16_t j = address; // NNVM pointer - - for (uint16_t i = 0; i < size; i++) { - buf[i] = nnvm[j++]; - if (!buf[i]) { - return; - } - } + uint16_t j = address; // NNVM pointer + + for (uint16_t i = 0; i < size; i++) { + buf[i] = nnvm[j++]; + if (!buf[i]) { + return; + } + } } void NNVM_WriteBytes(const uint16_t address, const int8_t *buf, const uint16_t size) { - uint16_t j = address; // NNVM pointer + uint16_t j = address; // NNVM pointer - for (uint16_t i = 0; i < size; i++) { - nnvm[j++] = buf[i]; - } - return; + for (uint16_t i = 0; i < size; i++) { + nnvm[j++] = buf[i]; + } + return; } void NNVM_ReadBytes(const uint16_t address, int8_t *buf, const uint16_t size) { - uint16_t j = address; // NNVM pointer + uint16_t j = address; // NNVM pointer - for (uint16_t i = 0; i < size; i++) { - buf[i] = nnvm[j++]; - } - return; + for (uint16_t i = 0; i < size; i++) { + buf[i] = nnvm[j++]; + } + return; } @@ -223,217 +223,217 @@ void NNVM_ReadBytes(const uint16_t address, int8_t *buf, const uint16_t size) /* * EEPROM_WriteString() - write string to EEPROM; may span multiple pages * - * This function writes a character string to IO mapped EEPROM. - * If memory mapped EEPROM is enabled this function will not work. - * This functiom will cancel all ongoing EEPROM page buffer loading - * operations, if any. - * - * A string may span multiple EEPROM pages. For each affected page it: - * - loads the page buffer from EEPROM with the contents for that page - * - writes the string bytes for that page into the page buffer - * - performs an atomic write for that page (erase and write) - * - does the next page until the string is complete. - * - * If 'terminate' is TRUE, add a null to the string, if FALSE, don't. - * - * Note that only the page buffer locations that have been copied into - * the page buffer (during string copy) will be affected when writing to - * the EEPROM (using AtomicPageWrite). Page buffer locations that have not - * been loaded will be left untouched in EEPROM. - * - * address must be between 0 and top-of-EEPROM (0x0FFF in a 256 or 192) - * string must be null-terminated - * terminate set TRUE to write string termination NULL to EEPROM - * set FALSE to not write NULL (useful for writing "files") - * - * returns pointer to next EEPROM location past the last byte written - * - * Background: The xmega EEPROM is rated at 100,000 operations (endurance). - * The endurance figure is dominated by the erase operation. Reads do not - * affect the endurance and writes have minimal effect. Erases occur at - * the page level, so a strategy to minimize page erases is needed. This - * function is a reasonably high endurance solution since erases only occur - * once for each time a string crosses a page, as opposed to once per byte - * written (as in EEPROM_WriteByte()). A slightly higher endurance solution - * would be to store up multiple contiguous string writes and perform them - * as single page operations. + * This function writes a character string to IO mapped EEPROM. + * If memory mapped EEPROM is enabled this function will not work. + * This functiom will cancel all ongoing EEPROM page buffer loading + * operations, if any. + * + * A string may span multiple EEPROM pages. For each affected page it: + * - loads the page buffer from EEPROM with the contents for that page + * - writes the string bytes for that page into the page buffer + * - performs an atomic write for that page (erase and write) + * - does the next page until the string is complete. + * + * If 'terminate' is TRUE, add a null to the string, if FALSE, don't. + * + * Note that only the page buffer locations that have been copied into + * the page buffer (during string copy) will be affected when writing to + * the EEPROM (using AtomicPageWrite). Page buffer locations that have not + * been loaded will be left untouched in EEPROM. + * + * address must be between 0 and top-of-EEPROM (0x0FFF in a 256 or 192) + * string must be null-terminated + * terminate set TRUE to write string termination 0 to EEPROM + * set FALSE to not write 0 (useful for writing "files") + * + * returns pointer to next EEPROM location past the last byte written + * + * Background: The xmega EEPROM is rated at 100,000 operations (endurance). + * The endurance figure is dominated by the erase operation. Reads do not + * affect the endurance and writes have minimal effect. Erases occur at + * the page level, so a strategy to minimize page erases is needed. This + * function is a reasonably high endurance solution since erases only occur + * once for each time a string crosses a page, as opposed to once per byte + * written (as in EEPROM_WriteByte()). A slightly higher endurance solution + * would be to store up multiple contiguous string writes and perform them + * as single page operations. */ uint16_t EEPROM_WriteString(const uint16_t address, const char *buf, const uint8_t terminate) { #ifdef __NNVM - NNVM_WriteString(address, buf, true); - return (address); + NNVM_WriteString(address, buf, true); + return address; #else - uint16_t addr = address; // local copy - uint8_t i = 0; // index into string - - EEPROM_DisableMapping(); - while (buf[i]) { - EEPROM_WriteByte(addr++, buf[i++]); - } - if (terminate) { - EEPROM_WriteByte(addr++, 0); - } - return (addr); // return next address in EEPROM + uint16_t addr = address; // local copy + uint8_t i = 0; // index into string + + EEPROM_DisableMapping(); + while (buf[i]) { + EEPROM_WriteByte(addr++, buf[i++]); + } + if (terminate) { + EEPROM_WriteByte(addr++, 0); + } + return addr; // return next address in EEPROM #endif //__NNVM } /* //+++ this is broken and will need to be fixed to work with NNVM #ifdef __TEST_EEPROM_WRITE - uint8_t testbuffer[32]; // fake out the page buffer -#endif // __TEST_EEPROM_WRITE + uint8_t testbuffer[32]; // fake out the page buffer +#endif // __TEST_EEPROM_WRITE uint16_t EEPROM_WriteString(const uint16_t address, const char *string, const uint8_t terminate) { - uint8_t i = 0; // index into string - uint16_t curaddr; // starting addr of string remaining to write - uint16_t endaddr; // ending address, adjusted for termination - uint16_t strnlen; // remaining unwritten string len (zero based) - uint8_t curpage; // current page number (0 - 127) - uint8_t endpage; // ending page number (0 - 127) - uint8_t byteidx; // index into page - uint8_t byteend; // ending byte number in page - - // initialize variables - EEPROM_DisableMapping(); - curaddr = address; - strnlen = strlen(buf) + terminate - 1; // terminate will be 1 or 0 - endaddr = address + strnlen; - curpage = (curaddr >> 5) & 0x7F; // mask it just to be safe - endpage = (endaddr >> 5) & 0x7F; // mask it just to be safe - - while (curpage <= endpage) { - // initialize addresses and variables for this write page - byteidx = curaddr & EEPROM_BYTE_ADDR_MASK_gm; - byteend = min((byteidx + strnlen), EEPROM_PAGESIZE-1); - strnlen = strnlen - (byteend - byteidx) - 1;// chars left in string - curaddr = curaddr + (byteend - byteidx) + 1;// bump current address - NVM.ADDR1 = curpage++; // set upper addr bytes - NVM.ADDR2 = 0x00; - - // load page buffer w/string contents and optional NULL termination - EEPROM_FlushBuffer(); // ensure no unintentional data is written - NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; // Page Load command - while (byteidx <= byteend) { + uint8_t i = 0; // index into string + uint16_t curaddr; // starting addr of string remaining to write + uint16_t endaddr; // ending address, adjusted for termination + uint16_t strnlen; // remaining unwritten string len (zero based) + uint8_t curpage; // current page number (0 - 127) + uint8_t endpage; // ending page number (0 - 127) + uint8_t byteidx; // index into page + uint8_t byteend; // ending byte number in page + + // initialize variables + EEPROM_DisableMapping(); + curaddr = address; + strnlen = strlen(buf) + terminate - 1; // terminate will be 1 or 0 + endaddr = address + strnlen; + curpage = (curaddr >> 5) & 0x7F; // mask it just to be safe + endpage = (endaddr >> 5) & 0x7F; // mask it just to be safe + + while (curpage <= endpage) { + // initialize addresses and variables for this write page + byteidx = curaddr & EEPROM_BYTE_ADDR_MASK_gm; + byteend = min((byteidx + strnlen), EEPROM_PAGESIZE-1); + strnlen = strnlen - (byteend - byteidx) - 1;// chars left in string + curaddr = curaddr + (byteend - byteidx) + 1;// bump current address + NVM.ADDR1 = curpage++; // set upper addr bytes + NVM.ADDR2 = 0x00; + + // load page buffer w/string contents and optional 0 termination + EEPROM_FlushBuffer(); // ensure no unintentional data is written + NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; // Page Load command + while (byteidx <= byteend) { #ifdef __NNVM - NVM.ADDR0 = byteidx; - testbuffer[byteidx++] = buf[i++]; + NVM.ADDR0 = byteidx; + testbuffer[byteidx++] = buf[i++]; #else - // use EEPROM (the real code) - NVM.ADDR0 = byteidx++; // set buffer location for data - NVM.DATA0 = buf[i++]; // writing DATA0 triggers write to buffer + // use EEPROM (the real code) + NVM.ADDR0 = byteidx++; // set buffer location for data + NVM.DATA0 = buf[i++]; // writing DATA0 triggers write to buffer #endif - } - // run EEPROM Atomic Write (Erase&Write) command. - NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc; // load command - NVM_EXEC_WRAPPER(); //write protection signature and execute cmd - } - return (curaddr); + } + // run EEPROM Atomic Write (Erase&Write) command. + NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc; // load command + NVM_EXEC_WRAPPER(); //write protection signature and execute cmd + } + return curaddr; } */ /* * EEPROM_ReadString() - read string from EEPROM; may span multiple pages * - * This function reads a character string to IO mapped EEPROM. - * If memory mapped EEPROM is enabled this function will not work. - * A string may span multiple EEPROM pages. + * This function reads a character string to IO mapped EEPROM. + * If memory mapped EEPROM is enabled this function will not work. + * A string may span multiple EEPROM pages. * - * address starting address of string in EEPROM space - * buf buffer to read string into - * size cutoff string and terminate at this length - * return next address past string termination + * address starting address of string in EEPROM space + * buf buffer to read string into + * size cutoff string and terminate at this length + * return next address past string termination */ uint16_t EEPROM_ReadString(const uint16_t address, char *buf, const uint16_t size) { #ifdef __NNVM - NNVM_ReadString(address, buf, size); - return(address + sizeof(buf)); + NNVM_ReadString(address, buf, size); + return(address + sizeof(buf)); #else - uint16_t addr = address; // local copy - uint16_t i = 0; // index into strings - - EEPROM_DisableMapping(); - - for (i = 0; i < size; i++) { - NVM.ADDR0 = addr & 0xFF; // set read address - NVM.ADDR1 = (addr++ >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - - EEPROM_WaitForNVM(); // Wait until NVM is not busy - NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command - NVM_EXEC(); - if (!(buf[i] = NVM.DATA0)) { - break; - } - } - if (i == size) { // null terinate the buffer overflow case - buf[i] = 0; - } - return (addr); + uint16_t addr = address; // local copy + uint16_t i = 0; // index into strings + + EEPROM_DisableMapping(); + + for (i = 0; i < size; i++) { + NVM.ADDR0 = addr & 0xFF; // set read address + NVM.ADDR1 = (addr++ >> 8) & EEPROM_ADDR1_MASK_gm; + NVM.ADDR2 = 0x00; + + EEPROM_WaitForNVM(); // Wait until NVM is not busy + NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command + NVM_EXEC(); + if (!(buf[i] = NVM.DATA0)) { + break; + } + } + if (i == size) { // null terinate the buffer overflow case + buf[i] = 0; + } + return addr; #endif //__NNVM } /* * EEPROM_WriteBytes() - write N bytes to EEPROM; may span multiple pages * - * This function writes a byte buffer to IO mapped EEPROM. - * If memory mapped EEPROM is enabled this function will not work. - * This functiom will cancel all ongoing EEPROM page buffer loading - * operations, if any. + * This function writes a byte buffer to IO mapped EEPROM. + * If memory mapped EEPROM is enabled this function will not work. + * This functiom will cancel all ongoing EEPROM page buffer loading + * operations, if any. * - * Returns address past the write + * Returns address past the write */ uint16_t EEPROM_WriteBytes(const uint16_t address, const int8_t *buf, const uint16_t size) { #ifdef __NNVM - NNVM_WriteBytes(address, buf, size); - return(address + size); + NNVM_WriteBytes(address, buf, size); + return(address + size); #else - uint16_t i; - uint16_t addr = address; // local copy - - EEPROM_DisableMapping(); - for (i=0; i> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - - EEPROM_WaitForNVM(); // Wait until NVM is not busy - NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command - NVM_EXEC(); - buf[i] = NVM.DATA0; - } - return (addr); + uint16_t i; + uint16_t addr = address; // local copy + + EEPROM_DisableMapping(); + + for (i=0; i> 8) & EEPROM_ADDR1_MASK_gm; + NVM.ADDR2 = 0x00; + + EEPROM_WaitForNVM(); // Wait until NVM is not busy + NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command + NVM_EXEC(); + buf[i] = NVM.DATA0; + } + return addr; #endif //__NNVM } @@ -448,13 +448,13 @@ uint16_t EEPROM_ReadBytes(const uint16_t address, int8_t *buf, const uint16_t si * * This function blocks waiting for any NVM access to finish including EEPROM * Use this function before any EEPROM accesses if you are not certain that - * any previous operations are finished yet, like an EEPROM write. + * any previous operations are finished yet, like an EEPROM write. */ void EEPROM_WaitForNVM( void ) { - do { - } while ((NVM.STATUS & NVM_NVMBUSY_bm) == NVM_NVMBUSY_bm); + do { + } while ((NVM.STATUS & NVM_NVMBUSY_bm) == NVM_NVMBUSY_bm); } /* @@ -469,66 +469,66 @@ void EEPROM_WaitForNVM( void ) void EEPROM_FlushBuffer( void ) { - EEPROM_WaitForNVM(); // Wait until NVM is not busy - if ((NVM.STATUS & NVM_EELOAD_bm) != 0) { // Flush page buffer if necessary - NVM.CMD = NVM_CMD_ERASE_EEPROM_BUFFER_gc; - NVM_EXEC(); - } + EEPROM_WaitForNVM(); // Wait until NVM is not busy + if ((NVM.STATUS & NVM_EELOAD_bm) != 0) { // Flush page buffer if necessary + NVM.CMD = NVM_CMD_ERASE_EEPROM_BUFFER_gc; + NVM_EXEC(); + } } /* - * EEPROM_WriteByte() - write one byte to EEPROM using IO mapping + * EEPROM_WriteByte() - write one byte to EEPROM using IO mapping * EEPROM_WriteByteByPage() - write one byte using page addressing (MACRO) * - * This function writes one byte to EEPROM using IO-mapped access. - * If memory mapped EEPROM is enabled this function will not work. + * This function writes one byte to EEPROM using IO-mapped access. + * If memory mapped EEPROM is enabled this function will not work. * This function flushes the EEPROM page buffers, and will cancel * any ongoing EEPROM page buffer loading operations, if any. * - * address EEPROM address, between 0 and EEPROM_SIZE - * value Byte value to write to EEPROM. + * address EEPROM address, between 0 and EEPROM_SIZE + * value Byte value to write to EEPROM. * - * Note: DO NOT USE THIS FUNCTION IF YOU CAN AVOID IT AS ENDURANCE SUCKS - * Use EEPROM_WriteString(), or write a new routine for binary blocks. + * Note: DO NOT USE THIS FUNCTION IF YOU CAN AVOID IT AS ENDURANCE SUCKS + * Use EEPROM_WriteString(), or write a new routine for binary blocks. */ void EEPROM_WriteByte(uint16_t address, uint8_t value) { - EEPROM_DisableMapping(); // *** SAFETY *** - EEPROM_FlushBuffer(); // prevent unintentional write - NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc;// load page_load command - NVM.ADDR0 = address & 0xFF; // set buffer addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.DATA0 = value; // load write data - triggers EEPROM page buffer load - NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc;// Atomic Write (Erase&Write) - NVM_EXEC_WRAPPER(); // Load command, write protection signature & exec command + EEPROM_DisableMapping(); // *** SAFETY *** + EEPROM_FlushBuffer(); // prevent unintentional write + NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc;// load page_load command + NVM.ADDR0 = address & 0xFF; // set buffer addresses + NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; + NVM.ADDR2 = 0x00; + NVM.DATA0 = value; // load write data - triggers EEPROM page buffer load + NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc;// Atomic Write (Erase&Write) + NVM_EXEC_WRAPPER(); // Load command, write protection signature & exec command } /* - * EEPROM_ReadByte() - Read one byte from EEPROM using IO mapping. - * EEPROM_ReadChar() - Read one char from EEPROM using IO mapping. + * EEPROM_ReadByte() - Read one byte from EEPROM using IO mapping. + * EEPROM_ReadChar() - Read one char from EEPROM using IO mapping. * EEPROM_ReadByteByPage() - Read one byte using page addressing * EEPROM_ReadCharByPage() - Read one char using page addressing * * This function reads one byte from EEPROM using IO-mapped access. * If memory mapped EEPROM is enabled, this function will not work. - * The other funcs are defined as macros. See the .h file. + * The other funcs are defined as macros. See the .h file. * - * address EEPROM address, between 0 and EEPROM_SIZE - * returns byte value read from EEPROM. + * address EEPROM address, between 0 and EEPROM_SIZE + * returns byte value read from EEPROM. */ uint8_t EEPROM_ReadByte(uint16_t address) { - EEPROM_DisableMapping(); // *** SAFETY *** - EEPROM_WaitForNVM(); // Wait until NVM is not busy - NVM.ADDR0 = address & 0xFF; // set read address - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command - NVM_EXEC(); - return NVM.DATA0; + EEPROM_DisableMapping(); // *** SAFETY *** + EEPROM_WaitForNVM(); // Wait until NVM is not busy + NVM.ADDR0 = address & 0xFF; // set read address + NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; + NVM.ADDR2 = 0x00; + NVM.CMD = NVM_CMD_READ_EEPROM_gc; // issue EEPROM Read command + NVM_EXEC(); + return NVM.DATA0; } /* @@ -550,13 +550,13 @@ uint8_t EEPROM_ReadByte(uint16_t address) void EEPROM_LoadByte(uint8_t byteAddr, uint8_t value) { - EEPROM_DisableMapping(); // +++ SAFETY - EEPROM_WaitForNVM(); // wait until NVM is not busy - NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; // prepare NVM command - NVM.ADDR0 = byteAddr & EEPROM_ADDR1_MASK_gm;// set address - NVM.ADDR1 = 0x00; - NVM.ADDR2 = 0x00; - NVM.DATA0 = value; // Set data, which triggers loading EEPROM page buffer + EEPROM_DisableMapping(); // +++ SAFETY + EEPROM_WaitForNVM(); // wait until NVM is not busy + NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; // prepare NVM command + NVM.ADDR0 = byteAddr & EEPROM_ADDR1_MASK_gm;// set address + NVM.ADDR1 = 0x00; + NVM.ADDR2 = 0x00; + NVM.DATA0 = value; // Set data, which triggers loading EEPROM page buffer } /* @@ -564,8 +564,8 @@ void EEPROM_LoadByte(uint8_t byteAddr, uint8_t value) * * This function loads an entire EEPROM page from an SRAM buffer to * the EEPROM page buffers. - * Make sure that the buffer is flushed before starting to load bytes. - * If memory mapped EEPROM is enabled this function will not work. + * Make sure that the buffer is flushed before starting to load bytes. + * If memory mapped EEPROM is enabled this function will not work. * * Note: Only the lower part of the address is used to address the buffer. * Therefore, no address parameter is needed. In the end, the data @@ -577,41 +577,41 @@ void EEPROM_LoadByte(uint8_t byteAddr, uint8_t value) void EEPROM_LoadPage( const uint8_t * values ) { - EEPROM_DisableMapping(); // +++ SAFETY - EEPROM_WaitForNVM(); // wait until NVM not busy - NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; - NVM.ADDR1 = 0x00; // set upper addr's to zero - NVM.ADDR2 = 0x00; - - for (uint8_t i = 0; i < EEPROM_PAGESIZE; ++i) { // load multiple bytes - NVM.ADDR0 = i; - NVM.DATA0 = *values; - ++values; - } + EEPROM_DisableMapping(); // +++ SAFETY + EEPROM_WaitForNVM(); // wait until NVM not busy + NVM.CMD = NVM_CMD_LOAD_EEPROM_BUFFER_gc; + NVM.ADDR1 = 0x00; // set upper addr's to zero + NVM.ADDR2 = 0x00; + + for (uint8_t i = 0; i < EEPROM_PAGESIZE; ++i) { // load multiple bytes + NVM.ADDR0 = i; + NVM.DATA0 = *values; + ++values; + } } /* * EEPROM_AtomicWritePage() - Write already loaded page into EEPROM. * - * This function writes the contents of an already loaded EEPROM page - * buffer into EEPROM memory. As this is an atomic write, the page in - * EEPROM will be erased automatically before writing. Note that only the - * page buffer locations that have been loaded will be used when writing - * to EEPROM. Page buffer locations that have not been loaded will be left - * untouched in EEPROM. + * This function writes the contents of an already loaded EEPROM page + * buffer into EEPROM memory. As this is an atomic write, the page in + * EEPROM will be erased automatically before writing. Note that only the + * page buffer locations that have been loaded will be used when writing + * to EEPROM. Page buffer locations that have not been loaded will be left + * untouched in EEPROM. * - * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE + * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE */ inline void EEPROM_AtomicWritePage(uint8_t pageAddr) { - EEPROM_WaitForNVM(); // wait until NVM not busy - uint16_t address = (uint16_t)(pageAddr*EEPROM_PAGESIZE); - NVM.ADDR0 = address & 0xFF; // set addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc; // erase & write page command - NVM_EXEC(); + EEPROM_WaitForNVM(); // wait until NVM not busy + uint16_t address = (uint16_t)(pageAddr*EEPROM_PAGESIZE); + NVM.ADDR0 = address & 0xFF; // set addresses + NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; + NVM.ADDR2 = 0x00; + NVM.CMD = NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc; // erase & write page command + NVM_EXEC(); } /* @@ -619,18 +619,18 @@ inline void EEPROM_AtomicWritePage(uint8_t pageAddr) * * This function erases one EEPROM page, so that every location reads 0xFF. * - * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE + * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE */ inline void EEPROM_ErasePage( uint8_t pageAddr ) { - EEPROM_WaitForNVM(); // wait until NVM not busy - uint16_t address = (uint16_t)(pageAddr*EEPROM_PAGESIZE); - NVM.ADDR0 = address & 0xFF; // set addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_ERASE_EEPROM_PAGE_gc; // erase page command - NVM_EXEC_WRAPPER(); + EEPROM_WaitForNVM(); // wait until NVM not busy + uint16_t address = (uint16_t)(pageAddr*EEPROM_PAGESIZE); + NVM.ADDR0 = address & 0xFF; // set addresses + NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; + NVM.ADDR2 = 0x00; + NVM.CMD = NVM_CMD_ERASE_EEPROM_PAGE_gc; // erase page command + NVM_EXEC_WRAPPER(); } /* @@ -638,20 +638,20 @@ inline void EEPROM_ErasePage( uint8_t pageAddr ) * * This function writes the contents of an already loaded EEPROM page * buffer into EEPROM memory. As this is a split write, the page in - * EEPROM will *NOT* be erased before writing. + * EEPROM will *NOT* be erased before writing. * - * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE + * pageAddr EEPROM Page address between 0 and EEPROM_SIZE/EEPROM_PAGESIZE */ inline void EEPROM_SplitWritePage( uint8_t pageAddr ) { - EEPROM_WaitForNVM(); // wait until NVM not busy - uint16_t address = (uint16_t)(pageAddr*EEPROM_PAGESIZE); - NVM.ADDR0 = address & 0xFF; // set addresses - NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; - NVM.ADDR2 = 0x00; - NVM.CMD = NVM_CMD_WRITE_EEPROM_PAGE_gc; // split write command - NVM_EXEC_WRAPPER(); + EEPROM_WaitForNVM(); // wait until NVM not busy + uint16_t address = (uint16_t)(pageAddr*EEPROM_PAGESIZE); + NVM.ADDR0 = address & 0xFF; // set addresses + NVM.ADDR1 = (address >> 8) & EEPROM_ADDR1_MASK_gm; + NVM.ADDR2 = 0x00; + NVM.CMD = NVM_CMD_WRITE_EEPROM_PAGE_gc; // split write command + NVM_EXEC_WRAPPER(); } /* @@ -660,8 +660,8 @@ inline void EEPROM_SplitWritePage( uint8_t pageAddr ) inline void EEPROM_EraseAll( void ) { - EEPROM_WaitForNVM(); // wait until NVM not busy - NVM.CMD = NVM_CMD_ERASE_EEPROM_gc; // erase all command - NVM_EXEC_WRAPPER(); + EEPROM_WaitForNVM(); // wait until NVM not busy + NVM.CMD = NVM_CMD_ERASE_EEPROM_gc; // erase all command + NVM_EXEC_WRAPPER(); } diff --git a/src/xmega/xmega_eeprom.h b/src/xmega/xmega_eeprom.h index 1e44d49..17bb0b8 100755 --- a/src/xmega/xmega_eeprom.h +++ b/src/xmega/xmega_eeprom.h @@ -3,14 +3,14 @@ * XMEGA EEPROM driver header file. * * This file contains the function prototypes and enumerator - * definitions for various configuration parameters for the - * XMEGA EEPROM driver. + * definitions for various configuration parameters for the + * XMEGA EEPROM driver. * * The driver is not intended for size and/or speed critical code, - * since most functions are just a few lines of code, and the - * function call overhead would decrease code performance. The driver - * is intended for rapid prototyping and documentation purposes for - * getting started with the XMEGA EEPROM module. + * since most functions are just a few lines of code, and the + * function call overhead would decrease code performance. The driver + * is intended for rapid prototyping and documentation purposes for + * getting started with the XMEGA EEPROM module. * * For size and/or speed critical code, it is recommended to copy the * function contents directly into your application instead of making @@ -21,7 +21,7 @@ * * Author: * \author * Original Author: Atmel Corporation: http://www.atmel.com \n - * Adapted by: Alden S. Hart Jr; 04/02/2010 + * Adapted by: Alden S. Hart Jr; 04/02/2010 * * Copyright (c) 2008, Atmel Corporation All rights reserved. * @@ -59,19 +59,19 @@ /* Configuration settings */ // UNCOMMENT FOR TEST ONLY - uses a RAM block to simulate EEPROM -//#define __NNVM // uncomment to use non-non-volatile RAM -#define NNVM_SIZE 2000 // size of emulation RAM block - xmega192/256 has 4096 +//#define __NNVM // uncomment to use non-non-volatile RAM +#define NNVM_SIZE 2000 // size of emulation RAM block - xmega192/256 has 4096 #ifndef MAPPED_EEPROM_START #define MAPPED_EEPROM_START 0x1000 #endif //__NNVM -#define EEPROM_PAGESIZE 32 // if this changes ...change below: -#define EEPROM_BYTE_ADDR_MASK_gm 0x1F // range of valid byte addrs in page -#define EEPROM_ADDR1_MASK_gm 0x0F // EEPROM is 4K = 0x0F, 2K = 0x07 +#define EEPROM_PAGESIZE 32 // if this changes ...change below: +#define EEPROM_BYTE_ADDR_MASK_gm 0x1F // range of valid byte addrs in page +#define EEPROM_ADDR1_MASK_gm 0x0F // EEPROM is 4K = 0x0F, 2K = 0x07 #define EEPROM(_pageAddr, _byteAddr) \ - ((uint8_t *) MAPPED_EEPROM_START)[_pageAddr*EEPROM_PAGESIZE + _byteAddr] + ((uint8_t *) MAPPED_EEPROM_START)[_pageAddr*EEPROM_PAGESIZE + _byteAddr] /* function prototypes for TinyG added functions */ uint16_t EEPROM_WriteString(const uint16_t address, const char *buf, const uint8_t terminate); @@ -80,7 +80,7 @@ uint16_t EEPROM_WriteBytes(const uint16_t address, const int8_t *buf, const uint uint16_t EEPROM_ReadBytes(const uint16_t address, int8_t *buf, const uint16_t size); //#ifdef __UNIT_TEST_EEPROM -void EEPROM_unit_tests(void); +void EEPROM_unit_tests(); //#endif /* Function prototypes for Atmel and Atmel-derived functions */ @@ -112,9 +112,9 @@ void EEPROM_EraseAll( void ); /* Enable EEPROM block sleep-when-not-used mode. * - * This macro enables power reduction mode for EEPROM. It means that - * the EEPROM block is disabled when not used. Note that there will be - * a penalty of 6 CPU cycles if EEPROM is accessed. + * This macro enables power reduction mode for EEPROM. It means that + * the EEPROM block is disabled when not used. Note that there will be + * a penalty of 6 CPU cycles if EEPROM is accessed. */ #define EEPROM_EnablePowerReduction() ( NVM.CTRLB |= NVM_EPRM_bm ) @@ -126,9 +126,9 @@ void EEPROM_EraseAll( void ); /* Enable EEPROM mapping into data space. * - * This macro enables mapping of EEPROM into data space. EEPROM starts at - * EEPROM_START in data memory. Read access can be done similar to ordinary - * SRAM access. + * This macro enables mapping of EEPROM into data space. EEPROM starts at + * EEPROM_START in data memory. Read access can be done similar to ordinary + * SRAM access. * * Note: This disables IO-mapped access to EEPROM, although page erase and * write operations still needs to be done through IO register. @@ -147,24 +147,24 @@ void EEPROM_EraseAll( void ); * * This macro sets the CCP register before setting the CMDEX bit in the * NVM.CTRLA register. The CMDEX bit must be set within 4 clock cycles - * after setting the protection byte in the CCP register. + * after setting the protection byte in the CCP register. */ /* -#define NVM_EXEC() asm("push r30" "\n\t" \ - "push r31" "\n\t" \ - "push r16" "\n\t" \ - "push r18" "\n\t" \ - "ldi r30, 0xCB" "\n\t" \ - "ldi r31, 0x01" "\n\t" \ - "ldi r16, 0xD8" "\n\t" \ - "ldi r18, 0x01" "\n\t" \ - "out 0x34, r16" "\n\t" \ - "st Z, r18" "\n\t" \ - "pop r18" "\n\t" \ - "pop r16" "\n\t" \ - "pop r31" "\n\t" \ - "pop r30" "\n\t" \ - ) +#define NVM_EXEC() asm("push r30" "\n\t" \ + "push r31" "\n\t" \ + "push r16" "\n\t" \ + "push r18" "\n\t" \ + "ldi r30, 0xCB" "\n\t" \ + "ldi r31, 0x01" "\n\t" \ + "ldi r16, 0xD8" "\n\t" \ + "ldi r18, 0x01" "\n\t" \ + "out 0x34, r16" "\n\t" \ + "st Z, r18" "\n\t" \ + "pop r18" "\n\t" \ + "pop r16" "\n\t" \ + "pop r31" "\n\t" \ + "pop r30" "\n\t" \ + ) */ /* ------------------------------------------------------------------------ * Modified version from jl_1978 - Feb 01, 2010 - 06:30 PM in thread: @@ -174,42 +174,42 @@ void EEPROM_EraseAll( void ); * complete which wakes the CPU back up. In your .c eeprom write/erase * functions, set the flag to enable sleep mode: * - * SLEEP.CTRL = 1; + * SLEEP.CTRL = 1; * * Oh yeah, and disable other interrupts. My system only uses low and * medium interrupts for other tasks. The high interrupt is only used * for the stuff above." */ /* -#define NVM_EXEC() asm("push r30" "\n\t" \ - "push r31" "\n\t" \ - "push r16" "\n\t" \ - "push r18" "\n\t" \ - "ldi r30, 0xCB" "\n\t" \ - "ldi r31, 0x01" "\n\t" \ - "ldi r16, 0xD8" "\n\t" \ - "ldi r18, 0x01" "\n\t" \ - "out 0x34, r16" "\n\t" \ - "st Z, r18" "\n\t" \ - "ldi r30, 0xCD" "\n\t" \ - "ldi r31, 0x01" "\n\t" \ - "ldi r18, 0x0C" "\n\t" \ - "st Z, r18" "\n\t" \ - "sleep" "\n\t" \ - "pop r18" "\n\t" \ - "pop r16" "\n\t" \ - "pop r31" "\n\t" \ - "pop r30" "\n\t" \ - ) +#define NVM_EXEC() asm("push r30" "\n\t" \ + "push r31" "\n\t" \ + "push r16" "\n\t" \ + "push r18" "\n\t" \ + "ldi r30, 0xCB" "\n\t" \ + "ldi r31, 0x01" "\n\t" \ + "ldi r16, 0xD8" "\n\t" \ + "ldi r18, 0x01" "\n\t" \ + "out 0x34, r16" "\n\t" \ + "st Z, r18" "\n\t" \ + "ldi r30, 0xCD" "\n\t" \ + "ldi r31, 0x01" "\n\t" \ + "ldi r18, 0x0C" "\n\t" \ + "st Z, r18" "\n\t" \ + "sleep" "\n\t" \ + "pop r18" "\n\t" \ + "pop r16" "\n\t" \ + "pop r31" "\n\t" \ + "pop r30" "\n\t" \ + ) */ //#define __UNIT_TEST_EEPROM #ifdef __UNIT_TEST_EEPROM -void EEPROM_unit_tests(void); -#define EEPROM_UNITS EEPROM_unit_tests(); +void EEPROM_unit_tests(); +#define EEPROM_UNITS EEPROM_unit_tests(); #else -#define EEPROM_UNITS +#define EEPROM_UNITS #endif #endif diff --git a/src/xmega/xmega_init.c b/src/xmega/xmega_init.c index 234d12f..0f1e1bc 100755 --- a/src/xmega/xmega_init.c +++ b/src/xmega/xmega_init.c @@ -24,15 +24,15 @@ #include "../hardware.h" #include "xmega_init.h" -void xmega_init_clocks(void); +void xmega_init_clocks(); void CCPWrite(volatile uint8_t * address, uint8_t value); /* * xmega_init() */ -void xmega_init(void) { - xmega_init_clocks(); +void xmega_init() { + xmega_init_clocks(); } /* @@ -42,48 +42,48 @@ void xmega_init(void) { * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 * Read the above thread to the bottom and you will find: * - OSC.XOSCCTRL = 0xCB; // 0.4-16 MHz XTAL - 16K CLK Start Up - OSC.CTRL = 0x08; // Enable External Oscillator - while(!testbit(OSC.STATUS,OSC_XOSCRDY_bp)); // wait until crystal stable - OSC.PLLCTRL = 0xC8; // XOSC is PLL Source - 8x Factor (128MHz) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - // Prescaler A=1, B=2, C=2 - // CLKPER4=128MHz, CLKPER2=64MHZ, CLKPER & CLKCPU = 32MHz - CCPWrite(&CLK.PSCTRL,0x03); - while(!testbit(OSC.STATUS,OSC_PLLRDY_bp)); // wait until PLL stable - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // Switch to PLL clock + OSC.XOSCCTRL = 0xCB; // 0.4-16 MHz XTAL - 16K CLK Start Up + OSC.CTRL = 0x08; // Enable External Oscillator + while(!testbit(OSC.STATUS,OSC_XOSCRDY_bp)); // wait until crystal stable + OSC.PLLCTRL = 0xC8; // XOSC is PLL Source - 8x Factor (128MHz) + OSC.CTRL = 0x18; // Enable PLL & External Oscillator + // Prescaler A=1, B=2, C=2 + // CLKPER4=128MHz, CLKPER2=64MHZ, CLKPER & CLKCPU = 32MHz + CCPWrite(&CLK.PSCTRL,0x03); + while(!testbit(OSC.STATUS,OSC_PLLRDY_bp)); // wait until PLL stable + CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // Switch to PLL clock */ -void xmega_init_clocks(void) +void xmega_init_clocks() { -#ifdef __CLOCK_EXTERNAL_8MHZ // external 8 Mhx Xtal with 4x PLL = 32 Mhz - OSC.XOSCCTRL = 0x4B; // 2-9 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup - OSC.CTRL = 0x08; // enable external crystal oscillator - while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - OSC.PLLCTRL = 0xC4; // XOSC is PLL Source; 4x Factor (32 MHz sys clock) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc);// switch to PLL clock - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock +#ifdef __CLOCK_EXTERNAL_8MHZ // external 8 Mhx Xtal with 4x PLL = 32 Mhz + OSC.XOSCCTRL = 0x4B; // 2-9 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup + OSC.CTRL = 0x08; // enable external crystal oscillator + while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + OSC.PLLCTRL = 0xC4; // XOSC is PLL Source; 4x Factor (32 MHz sys clock) + OSC.CTRL = 0x18; // Enable PLL & External Oscillator + while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc);// switch to PLL clock + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock #endif -#ifdef __CLOCK_EXTERNAL_16MHZ // external 16 Mhx Xtal with 2x PLL = 32 Mhz - OSC.XOSCCTRL = 0xCB; // 12-16 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup - OSC.CTRL = 0x08; // enable external crystal oscillator - while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - OSC.PLLCTRL = 0xC2; // XOSC is PLL Source; 2x Factor (32 MHz sys clock) - OSC.CTRL = 0x18; // Enable PLL & External Oscillator - while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready - CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc);// switch to PLL clock - OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock +#ifdef __CLOCK_EXTERNAL_16MHZ // external 16 Mhx Xtal with 2x PLL = 32 Mhz + OSC.XOSCCTRL = 0xCB; // 12-16 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup + OSC.CTRL = 0x08; // enable external crystal oscillator + while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + OSC.PLLCTRL = 0xC2; // XOSC is PLL Source; 2x Factor (32 MHz sys clock) + OSC.CTRL = 0x18; // Enable PLL & External Oscillator + while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready + CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc);// switch to PLL clock + OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock #endif -#ifdef __CLOCK_INTERNAL_32MHZ // 32 MHz internal clock (Boston Android code) - CCP = CCP_IOREG_gc; // Security Signature to modify clk - OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator - while(!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready - CCP = CCP_IOREG_gc; // Security Signature to modify clk - CLK.CTRL = 0x01; // select sysclock 32MHz osc +#ifdef __CLOCK_INTERNAL_32MHZ // 32 MHz internal clock (Boston Android code) + CCP = CCP_IOREG_gc; // Security Signature to modify clk + OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator + while(!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready + CCP = CCP_IOREG_gc; // Security Signature to modify clk + CLK.CTRL = 0x01; // select sysclock 32MHz osc #endif } @@ -140,49 +140,49 @@ void CCPWrite( volatile uint8_t * address, uint8_t value ) { #ifdef __ICCAVR__ - // Store global interrupt setting in scratch register and disable interrupts. + // Store global interrupt setting in scratch register and disable interrupts. asm("in R1, 0x3F \n" - "cli" - ); + "cli" + ); - // Move destination address pointer to Z pointer registers. - asm("movw r30, r16"); + // Move destination address pointer to Z pointer registers. + asm("movw r30, r16"); #ifdef RAMPZ - asm("ldi R16, 0 \n" + asm("ldi R16, 0 \n" "out 0x3B, R16" - ); + ); #endif - asm("ldi r16, 0xD8 \n" - "out 0x34, r16 \n" + asm("ldi r16, 0xD8 \n" + "out 0x34, r16 \n" #if (__MEMORY_MODEL__ == 1) - "st Z, r17 \n"); + "st Z, r17 \n"); #elif (__MEMORY_MODEL__ == 2) - "st Z, r18 \n"); + "st Z, r18 \n"); #else /* (__MEMORY_MODEL__ == 3) || (__MEMORY_MODEL__ == 5) */ - "st Z, r19 \n"); + "st Z, r19 \n"); #endif /* __MEMORY_MODEL__ */ - // Restore global interrupt setting from scratch register. + // Restore global interrupt setting from scratch register. asm("out 0x3F, R1"); #elif defined __GNUC__ - AVR_ENTER_CRITICAL_REGION(); - volatile uint8_t * tmpAddr = address; + AVR_ENTER_CRITICAL_REGION(); + volatile uint8_t * tmpAddr = address; #ifdef RAMPZ - RAMPZ = 0; + RAMPZ = 0; #endif - asm volatile( - "movw r30, %0" "\n\t" - "ldi r16, %2" "\n\t" - "out %3, r16" "\n\t" - "st Z, %1" "\n\t" - : - : "r" (tmpAddr), "r" (value), "M" (CCP_IOREG_gc), "i" (&CCP) - : "r16", "r30", "r31" - ); - - AVR_LEAVE_CRITICAL_REGION(); + asm volatile( + "movw r30, %0" "\n\t" + "ldi r16, %2" "\n\t" + "out %3, r16" "\n\t" + "st Z, %1" "\n\t" + : + : "r" (tmpAddr), "r" (value), "M" (CCP_IOREG_gc), "i" (&CCP) + : "r16", "r30", "r31" + ); + + AVR_LEAVE_CRITICAL_REGION(); #endif } diff --git a/src/xmega/xmega_init.h b/src/xmega/xmega_init.h index ee7ceb0..6588405 100755 --- a/src/xmega/xmega_init.h +++ b/src/xmega/xmega_init.h @@ -24,7 +24,7 @@ * Global Scope Functions */ -void xmega_init(void); +void xmega_init(); void CCPWrite( volatile uint8_t * address, uint8_t value ); #endif diff --git a/src/xmega/xmega_interrupts.c b/src/xmega/xmega_interrupts.c index 405656a..904b505 100755 --- a/src/xmega/xmega_interrupts.c +++ b/src/xmega/xmega_interrupts.c @@ -68,9 +68,9 @@ */ void PMIC_SetVectorLocationToBoot( void ) { - uint8_t temp = PMIC.CTRL | PMIC_IVSEL_bm; - CCP = CCP_IOREG_gc; - PMIC.CTRL = temp; + uint8_t temp = PMIC.CTRL | PMIC_IVSEL_bm; + CCP = CCP_IOREG_gc; + PMIC.CTRL = temp; } /*! \brief Move interrupt vector table to application area. @@ -82,7 +82,7 @@ void PMIC_SetVectorLocationToBoot( void ) */ void PMIC_SetVectorLocationToApplication( void ) { - uint8_t temp = PMIC.CTRL & ~PMIC_IVSEL_bm; - CCP = CCP_IOREG_gc; - PMIC.CTRL = temp; + uint8_t temp = PMIC.CTRL & ~PMIC_IVSEL_bm; + CCP = CCP_IOREG_gc; + PMIC.CTRL = temp; } diff --git a/src/xmega/xmega_interrupts.h b/src/xmega/xmega_interrupts.h index 88d1ee8..c15ef8f 100755 --- a/src/xmega/xmega_interrupts.h +++ b/src/xmega/xmega_interrupts.h @@ -67,7 +67,7 @@ #ifndef xmega_interrupts_h #define xmega_interrupts_h -#include // Was #include "avr_compiler.h" (ash mod) +#include // Was #include "avr_compiler.h" (ash mod) /* Definitions of macros. */ @@ -97,7 +97,7 @@ /*! \brief Enable round-robin scheduling for low-level interrupts. */ -#define PMIC_EnableRoundRobin() (PMIC.CTRL |= PMIC_RREN_bm) +#define PMIC_EnableRoundRobin() (PMIC.CTRL |= PMIC_RREN_bm) /*! \brief Disable round-robin scheduling for low-level interrupts. */ diff --git a/src/xmega/xmega_rtc.c b/src/xmega/xmega_rtc.c index 96bb401..58ef59a 100755 --- a/src/xmega/xmega_rtc.c +++ b/src/xmega/xmega_rtc.c @@ -25,7 +25,7 @@ #include "../switch.h" #include "xmega_rtc.h" -rtClock_t rtc; // allocate clock control struct +rtClock_t rtc; // allocate clock control struct /* * rtc_init() - initialize and start the clock @@ -35,22 +35,22 @@ rtClock_t rtc; // allocate clock control struct void rtc_init() { - OSC.CTRL |= OSC_RC32KEN_bm; // Turn on internal 32kHz. - do {} while ((OSC.STATUS & OSC_RC32KRDY_bm) == 0); // Wait for 32kHz oscillator to stabilize. - do {} while (RTC.STATUS & RTC_SYNCBUSY_bm); // Wait until RTC is not busy + OSC.CTRL |= OSC_RC32KEN_bm; // Turn on internal 32kHz. + do {} while ((OSC.STATUS & OSC_RC32KRDY_bm) == 0); // Wait for 32kHz oscillator to stabilize. + do {} while (RTC.STATUS & RTC_SYNCBUSY_bm); // Wait until RTC is not busy - CLK.RTCCTRL = CLK_RTCSRC_RCOSC_gc | CLK_RTCEN_bm; // Set internal 32kHz osc as RTC clock source - do {} while (RTC.STATUS & RTC_SYNCBUSY_bm); // Wait until RTC is not busy + CLK.RTCCTRL = CLK_RTCSRC_RCOSC_gc | CLK_RTCEN_bm; // Set internal 32kHz osc as RTC clock source + do {} while (RTC.STATUS & RTC_SYNCBUSY_bm); // Wait until RTC is not busy - // the following must be in this order or it doesn;t work - RTC.PER = RTC_MILLISECONDS-1; // set overflow period to 10ms - approximate - RTC.CNT = 0; - RTC.COMP = RTC_MILLISECONDS-1; - RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale (1x) - RTC.INTCTRL = RTC_COMPINTLVL; // interrupt on compare - rtc.rtc_ticks = 0; // reset tick counter - rtc.sys_ticks = 0; // reset tick counter - rtc.magic_end = MAGICNUM; + // the following must be in this order or it doesn;t work + RTC.PER = RTC_MILLISECONDS-1; // set overflow period to 10ms - approximate + RTC.CNT = 0; + RTC.COMP = RTC_MILLISECONDS-1; + RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale (1x) + RTC.INTCTRL = RTC_COMPINTLVL; // interrupt on compare + rtc.rtc_ticks = 0; // reset tick counter + rtc.sys_ticks = 0; // reset tick counter + rtc.magic_end = MAGICNUM; } /* @@ -62,18 +62,18 @@ void rtc_init() * Here's the code in case the main loop (non-interrupt) function needs to * create a critical region for variables set or used by the callback: * - * #include "gpio.h" - * #include "xmega_rtc.h" + * #include "gpio.h" + * #include "xmega_rtc.h" * - * RTC.INTCTRL = RTC_OVFINTLVL_OFF_gc; // disable interrupt - * blah blah blah critical region - * RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // enable interrupt + * RTC.INTCTRL = RTC_OVFINTLVL_OFF_gc; // disable interrupt + * blah blah blah critical region + * RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // enable interrupt */ ISR(RTC_COMP_vect) { - rtc.sys_ticks = ++rtc.rtc_ticks*10; // advance both tick counters as appropriate + rtc.sys_ticks = ++rtc.rtc_ticks*10; // advance both tick counters as appropriate - // callbacks to whatever you need to happen on each RTC tick go here: - switch_rtc_callback(); // switch debouncing + // callbacks to whatever you need to happen on each RTC tick go here: + switch_rtc_callback(); // switch debouncing } diff --git a/src/xmega/xmega_rtc.h b/src/xmega/xmega_rtc.h index d90b102..c0dea79 100755 --- a/src/xmega/xmega_rtc.h +++ b/src/xmega/xmega_rtc.h @@ -20,22 +20,22 @@ #ifndef XMEGA_RTC_H_ONCE #define XMEGA_RTC_H_ONCE -#define RTC_MILLISECONDS 10 // interrupt on every 10 RTC ticks (~10 ms) +#define RTC_MILLISECONDS 10 // interrupt on every 10 RTC ticks (~10 ms) // Interrupt level: pick one -#define RTC_COMPINTLVL RTC_COMPINTLVL_LO_gc; // lo interrupt on compare -//#define RTC_COMPINTLVL RTC_COMPINTLVL_MED_gc; // med interrupt on compare -//#define RTC_COMPINTLVL RTC_COMPINTLVL_HI_gc; // hi interrupt on compare +#define RTC_COMPINTLVL RTC_COMPINTLVL_LO_gc; // lo interrupt on compare +//#define RTC_COMPINTLVL RTC_COMPINTLVL_MED_gc; // med interrupt on compare +//#define RTC_COMPINTLVL RTC_COMPINTLVL_HI_gc; // hi interrupt on compare // Note: sys_ticks is in ms but is only accurate to 10 ms as it's derived from rtc_ticks typedef struct rtClock { - uint32_t rtc_ticks; // RTC tick counter, 10 uSec each - uint32_t sys_ticks; // system tick counter, 1 ms each - uint16_t magic_end; // magic number is read directly + uint32_t rtc_ticks; // RTC tick counter, 10 uSec each + uint32_t sys_ticks; // system tick counter, 1 ms each + uint16_t magic_end; // magic number is read directly } rtClock_t; extern rtClock_t rtc; -void rtc_init(void); // initialize and start general timer +void rtc_init(); // initialize and start general timer #endif // End of include guard: XMEGA_RTC_H_ONCE -- 2.27.0