From 81ab028307fe074a057cc811da42521b4a9f9355 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 29 Mar 2016 04:48:56 -0700 Subject: [PATCH] Cleaned up canonical machine and reduced memory usage --- data_usage.py | 36 +++ src/canonical_machine.c | 634 ++++++++++++++++++---------------------- src/canonical_machine.h | 562 +++++++++++++---------------------- src/config.h | 45 ++- src/cycle_homing.c | 18 +- src/cycle_probing.c | 13 +- src/gcode_parser.c | 43 +-- src/plan/arc.c | 50 ++-- src/plan/buffer.h | 2 +- src/plan/command.c | 4 +- src/plan/dwell.c | 4 +- src/plan/exec.c | 42 +-- src/plan/feedhold.c | 2 +- src/plan/line.c | 140 +-------- src/plan/line.h | 2 +- src/plan/planner.c | 6 +- src/plan/planner.h | 4 +- src/report.c | 3 +- src/spindle.c | 30 +- src/spindle.h | 11 +- src/util.h | 2 +- 21 files changed, 691 insertions(+), 962 deletions(-) create mode 100755 data_usage.py diff --git a/data_usage.py b/data_usage.py new file mode 100755 index 0000000..8ba92f0 --- /dev/null +++ b/data_usage.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 + +import os +import re +import shlex +import subprocess + + +lineRE = r'%(addr)s ' +command = 'avr-objdump -j .bss -t buildbotics.elf' + +proc = subprocess.Popen(shlex.split(command), stdout = subprocess.PIPE) + +out, err = proc.communicate() + +if proc.returncode: + print(out) + raise Exception('command failed') + +def get_sizes(data): + for line in data.decode().split('\n'): + if not re.match(r'^[0-9a-f]{8} .*', line): continue + + size, name = int(line[22:30], 16), line[31:] + if not size: continue + + yield (size, name) + +sizes = sorted(get_sizes(out)) +total = sum(x[0] for x in sizes) + +for size, name in sizes: + print('% 6d %5.2f%% %s' % (size, size / total * 100, name)) + +print('-' * 40) +print('% 6d Total' % total) diff --git a/src/canonical_machine.c b/src/canonical_machine.c index a113fe2..3aaee6e 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -35,39 +35,8 @@ * executes commands - passing the stateless commands to the motion * planning layer. * - * System state contexts - Gcode models - * - * Useful reference for doing C callbacks - * http://www.newty.de/fpt/fpt.html - * - * There are 3 temporal contexts for system state: - The gcode - * model in the canonical machine (the MODEL context, held in gm) - - * The gcode model used by the planner (PLANNER context, held in - * bf's and mm) - The gcode model used during motion for reporting - * (RUNTIME context, held in mr) - * - * It's a bit more complicated than this. The 'gm' struct contains - * the core Gcode model context. This originates in the canonical - * machine and is copied to each planner buffer (bf buffer) during - * motion planning. Finally, the gm context is passed to the - * runtime (mr) for the RUNTIME context. So at last count the Gcode - * model exists in as many as 30 copies in the system. (1+28+1) - * - * Depending on the need, any one of these contexts may be called - * for reporting or by a function. Most typically, all new commends - * from the gcode parser work form the MODEL context, and status - * reports pull from the RUNTIME while in motion, and from MODEL - * when at rest. A convenience is provided in the ACTIVE_MODEL - * pointer to point to the right context. - * * 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. - * * "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 @@ -123,18 +92,6 @@ cmSingleton_t cm = { - .junction_acceleration = JUNCTION_ACCELERATION, - .chordal_tolerance = CHORDAL_TOLERANCE, - .soft_limit_enable = SOFT_LIMIT_ENABLE, - - .arc_segment_len = ARC_SEGMENT_LENGTH, - - .coord_system = GCODE_DEFAULT_COORD_SYSTEM, - .select_plane = GCODE_DEFAULT_PLANE, - .units_mode = GCODE_DEFAULT_UNITS, - .path_control = GCODE_DEFAULT_PATH_CONTROL, - .distance_mode = GCODE_DEFAULT_DISTANCE_MODE, - // Offsets .offset = { {}, // ABSOLUTE_COORDS @@ -239,7 +196,6 @@ cmSingleton_t cm = { // State .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE}, - .gmx = {.block_delete_switch = true}, .gn = {}, .gf = {}, }; @@ -257,7 +213,7 @@ static void _exec_program_finalize(float *value, float *flag); // Canonical Machine State functions /// Combines raw states into something a user might want to see -uint8_t cm_get_combined_state() { +cmCombinedState_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; @@ -272,114 +228,75 @@ uint8_t cm_get_combined_state() { 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;} +uint32_t cm_get_linenum() {return cm.gm.linenum;} +cmMachineState_t cm_get_machine_state() {return cm.machine_state;} +cmCycleState_t cm_get_cycle_state() {return cm.cycle_state;} +cmMotionState_t cm_get_motion_state() {return cm.motion_state;} +cmFeedholdState_t cm_get_hold_state() {return cm.hold_state;} +cmHomingState_t cm_get_homing_state() {return cm.homing_state;} +cmMotionMode_t cm_get_motion_mode() {return cm.gm.motion_mode;} +cmCoordSystem_t cm_get_coord_system() {return cm.gm.coord_system;} +cmUnitsMode_t cm_get_units_mode() {return cm.gm.units_mode;} +cmCanonicalPlane_t cm_get_select_plane() {return cm.gm.select_plane;} +cmPathControlMode_t cm_get_path_control() {return cm.gm.path_control;} +cmDistanceMode_t cm_get_distance_mode() {return cm.gm.distance_mode;} +cmFeedRateMode_t cm_get_feed_rate_mode() {return cm.gm.feed_rate_mode;} +uint8_t cm_get_tool() {return cm.gm.tool;} +cmSpindleMode_t cm_get_spindle_mode() {return cm.gm.spindle_mode;} +bool cm_get_runtime_busy() {return mp_get_runtime_busy();} +float cm_get_feed_rate() {return cm.gm.feed_rate;} /// Adjusts active model pointer as well -void cm_set_motion_state(uint8_t motion_state) { +void cm_set_motion_state(cmMotionState_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, PLANNER, RUNTIME, ACTIVE_MODEL -uint32_t cm_get_linenum(GCodeState_t *gcode_state) { - return gcode_state->linenum; +void cm_set_motion_mode(cmMotionMode_t motion_mode) { + cm.gm.motion_mode = motion_mode; } -uint8_t cm_get_motion_mode(GCodeState_t *gcode_state) { - return gcode_state->motion_mode; +void cm_set_spindle_mode(cmSpindleMode_t spindle_mode) { + cm.gm.spindle_mode = spindle_mode; } -uint8_t cm_get_coord_system(GCodeState_t *gcode_state) { - return gcode_state->coord_system; -} - - -uint8_t cm_get_units_mode(GCodeState_t *gcode_state) { - return gcode_state->units_mode; -} - - -uint8_t cm_get_select_plane(GCodeState_t *gcode_state) { - return gcode_state->select_plane; -} - - -uint8_t cm_get_path_control(GCodeState_t *gcode_state) { - return gcode_state->path_control; -} +void cm_set_spindle_speed_parameter(float speed) {cm.gm.spindle_speed = speed;} +void cm_set_tool_number(uint8_t tool) {cm.gm.tool = tool;} -uint8_t cm_get_distance_mode(GCodeState_t *gcode_state) { - return gcode_state->distance_mode; -} - - -uint8_t cm_get_feed_rate_mode(GCodeState_t *gcode_state) { - return gcode_state->feed_rate_mode; -} - - -uint8_t cm_get_tool(GCodeState_t *gcode_state) {return gcode_state->tool;} - - -uint8_t cm_get_spindle_mode(GCodeState_t *gcode_state) { - return gcode_state->spindle_mode; -} - - -uint8_t cm_get_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; -} - - -void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode) { - gcode_state->motion_mode = motion_mode; +void cm_set_absolute_override(bool absolute_override) { + cm.gm.absolute_override = absolute_override; + // must reset offsets if you change absolute override + cm_set_work_offsets(); } -void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode) { - gcode_state->spindle_mode = spindle_mode; -} +void cm_set_model_linenum(uint32_t linenum) {cm.gm.linenum = linenum;} -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; -} - +/* Jerk functions + * + * 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 axis_jerk() functions expect the jerk in divided-by 1,000,000 form + */ -void cm_set_absolute_override(GCodeState_t *gcode_state, - uint8_t absolute_override) { - gcode_state->absolute_override = absolute_override; - // must reset offsets if you change absolute override - cm_set_work_offsets(MODEL); +/// returns jerk for an axis +float cm_get_axis_jerk(uint8_t axis) { + return cm.a[axis].jerk_max; } -void cm_set_model_linenum(uint32_t linenum) { - cm.gm.linenum = linenum; // you must first set the model line number, +/// sets the jerk for an axis, including recirpcal and cached values +void cm_set_axis_jerk(uint8_t axis, float jerk) { + cm.a[axis].jerk_max = jerk; + cm.a[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); } @@ -430,67 +347,46 @@ float cm_get_active_coord_offset(uint8_t axis) { if (cm.gm.absolute_override) return 0; // no offset in absolute override mode float offset = cm.offset[cm.gm.coord_system][axis]; - if (cm.gmx.origin_offset_enable) - offset += cm.gmx.origin_offset[axis]; // includes G5x and G92 components + if (cm.origin_offset_enable) + offset += cm.origin_offset[axis]; // includes G5x and G92 components return offset; } -/* Return a coord offset from the gcode_state - * - * This function accepts as input: MODEL, PLANNER, RUNTIME, ACTIVE_MODEL - */ -float cm_get_work_offset(GCodeState_t *gcode_state, uint8_t axis) { - return gcode_state->work_offset[axis]; +/// Return a coord offset +float cm_get_work_offset(uint8_t axis) { + return cm.ms.work_offset[axis]; } -/* Capture coord offsets from the model into absolute values in the gcode_state - * - * This function accepts as input: MODEL, PLANNER, RUNTIME, ACTIVE_MODEL - */ -void cm_set_work_offsets(GCodeState_t *gcode_state) { - for (uint8_t axis = AXIS_X; axis < AXES; axis++) - gcode_state->work_offset[axis] = cm_get_active_coord_offset(axis); +// Capture coord offsets from the model into absolute values +void cm_set_work_offsets() { + for (int axis = 0; axis < AXES; axis++) + cm.ms.work_offset[axis] = cm_get_active_coord_offset(axis); } /* Get position of axis in absolute coordinates * - * This function accepts as input: MODEL, RUNTIME - * - * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's) * NOTE: Machine position is always returned in mm mode. No units conversion * is performed */ -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(uint8_t axis) { + return cm.position[axis]; } -/* Return work position in external form +/* Return work position 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 + * NOTE: This function only works after the gcode_state struct has had the * work_offsets setup by calling cm_get_model_coord_offset_vector() first. - * - * This function accepts as input: - * MODEL - * RUNTIME - * - * 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(uint8_t axis) { + float position = cm.position[axis] - cm_get_active_coord_offset(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; + if (cm.gm.units_mode == INCHES) position /= MM_PER_INCH; return position; } @@ -514,7 +410,7 @@ float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis) { * close to the starting point. */ void cm_finalize_move() { - copy_vector(cm.gmx.position, cm.gm.target); // update model position + copy_vector(cm.position, cm.ms.target); // update model position // if in ivnerse time mode reset feed rate so next block requires an // explicit feed rate setting @@ -524,9 +420,117 @@ void cm_finalize_move() { } +/* Compute optimal and minimum move times into the gcode_state + * + * "Minimum time" is the fastest the move can be performed given + * the velocity constraints on each participating axis - regardless + * of the feed rate requested. The minimum time is the time limited + * by the rate-limiting axis. The minimum time is needed to compute + * the optimal time and is recorded for possible feed override + * computation. + * + * "Optimal time" is either the time resulting from the requested + * feed rate or the minimum time if the requested feed rate is not + * achievable. Optimal times for traverses are always the minimum + * time. + * + * The gcode state must have targets set prior by having + * cm_set_target(). Axis modes are taken into account by this. + * + * The following times are compared and the longest is returned: + * - G93 inverse time (if G93 is active) + * - time for coordinated move at requested feed rate + * - time that the slowest axis would require for the move + * + * Sets the following variables in the gcode_state struct + * - move_time is set to optimal time + * + * 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. + */ +void cm_calc_move_time(const float axis_length[], const float axis_square[]) { + float inv_time = 0; // inverse time if doing a feed in G93 mode + float xyz_time = 0; // linear coordinated move at requested feed + float abc_time = 0; // rotary coordinated move at requested feed + float max_time = 0; // time required for the rate-limiting axis + float tmp_time = 0; // used in computation + + // compute times for feed motion + if (cm.gm.motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) { + if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { + // feed rate was un-inverted to minutes by cm_set_feed_rate() + inv_time = cm.gm.feed_rate; + cm.gm.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]) / cm.gm.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]) / cm.gm.feed_rate; + } + } + + for (uint8_t axis = 0; axis < AXES; axis++) { + if (cm.gm.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); + } + + cm.ms.move_time = max4(inv_time, max_time, xyz_time, abc_time); +} + + /// Set endpoint position from final runtime position void cm_update_model_position_from_runtime() { - copy_vector(cm.gmx.position, mr.gm.target); + copy_vector(cm.position, mr.ms.target); } @@ -560,38 +564,37 @@ static float _calc_ABC(uint8_t axis, float target[], float flag[]) { cm.a[axis].axis_mode == AXIS_INHIBITED) return target[axis]; // no mm conversion - it's in degrees - return _to_millimeters(target[axis]) * 360 / (2 * M_PI * cm.a[axis].radius); + return TO_MILLIMETERS(target[axis]) * 360 / (2 * M_PI * cm.a[axis].radius); } void cm_set_model_target(float target[], float flag[]) { - uint8_t axis; float tmp = 0; // process XYZABC for lower modes - for (axis = AXIS_X; axis <= AXIS_Z; axis++) { + for (int axis = AXIS_X; axis <= AXIS_Z; axis++) { if (fp_FALSE(flag[axis]) || cm.a[axis].axis_mode == AXIS_DISABLED) continue; // skip axis if not flagged for update or its disabled else if (cm.a[axis].axis_mode == AXIS_STANDARD || cm.a[axis].axis_mode == AXIS_INHIBITED) { if (cm.gm.distance_mode == ABSOLUTE_MODE) - cm.gm.target[axis] = - cm_get_active_coord_offset(axis) + _to_millimeters(target[axis]); - else cm.gm.target[axis] += _to_millimeters(target[axis]); + cm.ms.target[axis] = + cm_get_active_coord_offset(axis) + TO_MILLIMETERS(target[axis]); + else cm.ms.target[axis] += TO_MILLIMETERS(target[axis]); } } // FYI: The ABC loop below relies on the XYZ loop having been run first - for (axis = AXIS_A; axis <= AXIS_C; axis++) { + for (int axis = AXIS_A; axis <= AXIS_C; axis++) { if (fp_FALSE(flag[axis]) || cm.a[axis].axis_mode == AXIS_DISABLED) continue; // skip axis if not flagged for update or its disabled else tmp = _calc_ABC(axis, target, flag); if (cm.gm.distance_mode == ABSOLUTE_MODE) // sacidu93's fix to Issue #22 - cm.gm.target[axis] = tmp + cm_get_active_coord_offset(axis); - else cm.gm.target[axis] += tmp; + cm.ms.target[axis] = tmp + cm_get_active_coord_offset(axis); + else cm.ms.target[axis] += tmp; } } @@ -608,8 +611,8 @@ void cm_set_model_target(float target[], float flag[]) { * be tested w/the other disabled, should that requirement ever arise. */ stat_t cm_test_soft_limits(float target[]) { - if (cm.soft_limit_enable) - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { +#ifdef SOFT_LIMIT_ENABLE + for (int axis = 0; axis < AXES; axis++) { if (!cm.homed[axis]) continue; // don't test axes that are not homed if (fp_EQ(cm.a[axis].travel_min, cm.a[axis].travel_max)) continue; @@ -622,6 +625,7 @@ stat_t cm_test_soft_limits(float target[]) { target[axis] > cm.a[axis].travel_max) return STAT_SOFT_LIMIT_EXCEEDED; } +#endif return STAT_OK; } @@ -637,20 +641,17 @@ stat_t cm_test_soft_limits(float target[]) { // Initialization and Termination (4.3.2) -/// Config init cfg_init() must have been run beforehand void canonical_machine_init() { - ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer - // Init 1/jerk for (uint8_t axis = 0; axis < AXES; axis++) cm.a[axis].recip_jerk = 1 / (cm.a[axis].jerk_max * JERK_MULTIPLIER); // 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_units_mode(GCODE_DEFAULT_UNITS); + cm_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); + cm_set_plane(GCODE_DEFAULT_PLANE); + cm_set_path_control(GCODE_DEFAULT_PATH_CONTROL); + cm_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default // Sub-system inits @@ -694,27 +695,18 @@ stat_t cm_hard_alarm(stat_t status) { // These functions assume input validation occurred upstream. /// G17, G18, G19 select axis plane -stat_t cm_select_plane(uint8_t plane) { - cm.gm.select_plane = plane; - return STAT_OK; -} +void cm_set_plane(cmCanonicalPlane_t plane) {cm.gm.select_plane = plane;} /// G20, G21 -stat_t cm_set_units_mode(uint8_t mode) { - cm.gm.units_mode = mode; // 0 = inches, 1 = mm. - return STAT_OK; -} +void cm_set_units_mode(cmUnitsMode_t mode) {cm.gm.units_mode = mode;} /// G90, G91 -stat_t cm_set_distance_mode(uint8_t mode) { - cm.gm.distance_mode = mode; // 0 = absolute mode, 1 = incremental - return STAT_OK; -} +void cm_set_distance_mode(cmDistanceMode_t mode) {cm.gm.distance_mode = mode;} -/* G10 L2 Pn, affects MODEL only, delayed persistence +/* G10 L2 Pn, delayed persistence * * This function applies the offset to the GM model. You can also * use $g54x - $g59c config functions to change offsets. @@ -723,16 +715,14 @@ stat_t cm_set_distance_mode(uint8_t mode) { * accomplished by calling cm_set_work_offsets() immediately * afterwards. */ -stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], +void cm_set_coord_offsets(cmCoordSystem_t coord_system, float offset[], float flag[]) { if (coord_system < G54 || coord_system > COORD_SYSTEM_MAX) - return STAT_INPUT_VALUE_RANGE_ERROR; // you can't set G53 + return; // you can't set G53 - for (uint8_t axis = AXIS_X; axis < AXES; axis++) + for (int axis = 0; axis < AXES; axis++) if (fp_TRUE(flag[axis])) - cm.offset[coord_system][axis] = _to_millimeters(offset[axis]); - - return STAT_OK; + cm.offset[coord_system][axis] = TO_MILLIMETERS(offset[axis]); } @@ -740,27 +730,27 @@ stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], // (synchronous) /// G54-G59 -stat_t cm_set_coord_system(uint8_t coord_system) { +void cm_set_coord_system(cmCoordSystem_t coord_system) { cm.gm.coord_system = coord_system; // pass coordinate system in value[0] element - float value[AXES] = {(float)coord_system, 0, 0, 0, 0, 0}; + float value[AXES] = {coord_system}; // second vector (flags) is not used, so fake it mp_queue_command(_exec_offset, value, value); - return STAT_OK; } static void _exec_offset(float *value, float *flag) { // coordinate system is passed in value[0] element - uint8_t coord_system = ((uint8_t)value[0]); + uint8_t coord_system = value[0]; float offsets[AXES]; - for (uint8_t axis = AXIS_X; axis < AXES; axis++) + + for (int axis = 0; axis < AXES; axis++) offsets[axis] = cm.offset[coord_system][axis] + - (cm.gmx.origin_offset[axis] * cm.gmx.origin_offset_enable); + cm.origin_offset[axis] * (cm.origin_offset_enable ? 1 : 0); mp_set_runtime_work_offset(offsets); - cm_set_work_offsets(MODEL); // set work offsets in the Gcode model + cm_set_work_offsets(); // set work offsets in the Gcode model } @@ -781,10 +771,10 @@ static void _exec_offset(float *value, float *flag) { * 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) { +void cm_set_position(int axis, float position) { // TODO: Interlock involving runtime_busy test - cm.gmx.position[axis] = position; - cm.gm.target[axis] = position; + cm.position[axis] = position; + cm.ms.target[axis] = position; mp_set_planner_position(axis, position); mp_set_runtime_position(axis, position); mp_set_steps_to_runtime_position(); @@ -805,25 +795,23 @@ void cm_set_position(uint8_t axis, float position) { * 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[]) { +void cm_set_absolute_origin(float origin[], float flag[]) { float value[AXES]; - for (uint8_t axis = AXIS_X; axis < AXES; axis++) + for (int axis = 0; 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 + value[axis] = TO_MILLIMETERS(origin[axis]); + cm.position[axis] = value[axis]; // set model position + cm.ms.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++) + for (int axis = 0; 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 here @@ -838,54 +826,46 @@ static void _exec_absolute_origin(float *value, float *flag) { */ /// G92 -stat_t cm_set_origin_offsets(float offset[], float flag[]) { +void 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++) + cm.origin_offset_enable = true; + for (int axis = 0; 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]); + cm.origin_offset[axis] = cm.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 // pass coordinate system in value[0] element float value[AXES] = {cm.gm.coord_system}; mp_queue_command(_exec_offset, value, value); // second vector is not used - - return STAT_OK; } /// G92.1 -stat_t cm_reset_origin_offsets() { - cm.gmx.origin_offset_enable = 0; - for (uint8_t axis = AXIS_X; axis < AXES; axis++) - cm.gmx.origin_offset[axis] = 0; +void cm_reset_origin_offsets() { + cm.origin_offset_enable = false; + for (int axis = 0; axis < AXES; axis++) + cm.origin_offset[axis] = 0; float value[AXES] = {cm.gm.coord_system}; mp_queue_command(_exec_offset, value, value); - - return STAT_OK; } /// G92.2 -stat_t cm_suspend_origin_offsets() { - cm.gmx.origin_offset_enable = 0; +void cm_suspend_origin_offsets() { + cm.origin_offset_enable = false; float value[AXES] = {cm.gm.coord_system}; mp_queue_command(_exec_offset, value, value); - - return STAT_OK; } /// G92.3 -stat_t cm_resume_origin_offsets() { - cm.gmx.origin_offset_enable = 1; +void cm_resume_origin_offsets() { + cm.origin_offset_enable = true; float value[AXES] = {cm.gm.coord_system}; mp_queue_command(_exec_offset, value, value); - - return STAT_OK; } @@ -897,13 +877,13 @@ stat_t cm_straight_traverse(float target[], float flags[]) { cm_set_model_target(target, flags); // test soft limits - stat_t status = cm_test_soft_limits(cm.gm.target); + stat_t status = cm_test_soft_limits(cm.ms.target); if (status != STAT_OK) return cm_soft_alarm(status); // prep and plan the move cm_set_work_offsets(&cm.gm); // capture fully resolved offsets to the state cm_cycle_start(); // required for homing & other cycles - mp_aline(&cm.gm); // send the move to the planner + mp_aline(&cm.ms); // send the move to the planner cm_finalize_move(); return STAT_OK; @@ -911,15 +891,12 @@ stat_t cm_straight_traverse(float target[], float flags[]) { /// G28.1 -stat_t cm_set_g28_position() { - copy_vector(cm.gmx.g28_position, cm.gmx.position); - return STAT_OK; -} +void cm_set_g28_position() {copy_vector(cm.g28_position, cm.position);} /// G28 stat_t cm_goto_g28_position(float target[], float flags[]) { - cm_set_absolute_override(MODEL, true); + cm_set_absolute_override(true); // move through intermediate point, or skip cm_straight_traverse(target, flags); @@ -928,64 +905,52 @@ stat_t cm_goto_g28_position(float target[], float flags[]) { // execute actual stored move float f[] = {1, 1, 1, 1, 1, 1}; - return cm_straight_traverse(cm.gmx.g28_position, f); + return cm_straight_traverse(cm.g28_position, f); } /// G30.1 -stat_t cm_set_g30_position() { - copy_vector(cm.gmx.g30_position, cm.gmx.position); - return STAT_OK; -} +void cm_set_g30_position() {copy_vector(cm.g30_position, cm.position);} /// G30 stat_t cm_goto_g30_position(float target[], float flags[]) { - cm_set_absolute_override(MODEL, true); + cm_set_absolute_override(true); // move through intermediate point, or skip cm_straight_traverse(target, flags); // make sure you have an available buffer while (!mp_get_planner_buffers_available()); float f[] = {1, 1, 1, 1, 1, 1}; // execute actual stored move - return cm_straight_traverse(cm.gmx.g30_position, f); + return cm_straight_traverse(cm.g30_position, f); } // Machining Attributes (4.3.5) -/// F parameter (affects MODEL only) +/// F parameter /// Normalize feed rate to mm/min or to minutes if in inverse time mode -stat_t cm_set_feed_rate(float feed_rate) { +void cm_set_feed_rate(float feed_rate) { if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) // normalize to minutes (active for this gcode block only) cm.gm.feed_rate = 1 / feed_rate; - else cm.gm.feed_rate = _to_millimeters(feed_rate); - - return STAT_OK; + else cm.gm.feed_rate = TO_MILLIMETERS(feed_rate); } -/// G93, G94 (affects MODEL only) See cmFeedRateMode -stat_t cm_set_feed_rate_mode(uint8_t mode) { - cm.gm.feed_rate_mode = mode; - return STAT_OK; -} +/// G93, G94 See cmFeedRateMode +void cm_set_feed_rate_mode(cmFeedRateMode_t mode) {cm.gm.feed_rate_mode = mode;} -/// G61, G61.1, G64 (affects MODEL only) -stat_t cm_set_path_control(uint8_t mode) { - cm.gm.path_control = mode; - return STAT_OK; -} +/// G61, G61.1, G64 +void cm_set_path_control(cmPathControlMode_t mode) {cm.gm.path_control = mode;} // Machining Functions (4.3.6) See arc.c /// G4, P parameter (seconds) stat_t cm_dwell(float seconds) { - cm.gm.parameter = seconds; return mp_dwell(seconds); } @@ -1000,13 +965,13 @@ stat_t cm_straight_feed(float target[], float flags[]) { cm_set_model_target(target, flags); // test soft limits - stat_t status = cm_test_soft_limits(cm.gm.target); + stat_t status = cm_test_soft_limits(cm.ms.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 state cm_cycle_start(); // required for homing & other cycles - status = mp_aline(&cm.gm); // send the move to the planner + status = mp_aline(&cm.ms); // send the move to the planner cm_finalize_move(); return status; @@ -1022,23 +987,21 @@ stat_t cm_straight_feed(float target[], float flags[]) { */ /// T parameter -stat_t cm_select_tool(uint8_t tool_select) { +void cm_select_tool(uint8_t tool_select) { float value[AXES] = {tool_select}; 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]; + cm.gm.tool_select = value[0]; } /// M6 This might become a complete tool change cycle -stat_t cm_change_tool(uint8_t tool_change) { +void cm_change_tool(uint8_t tool_change) { float value[AXES] = {cm.gm.tool_select}; mp_queue_command(_exec_change_tool, value, value); - return STAT_OK; } @@ -1049,15 +1012,14 @@ static void _exec_change_tool(float *value, float *flag) { // Miscellaneous Functions (4.3.9) /// M7 -stat_t cm_mist_coolant_control(uint8_t mist_coolant) { - float value[AXES] = {(float)mist_coolant, 0, 0, 0, 0, 0}; +void cm_mist_coolant_control(bool mist_coolant) { + float value[AXES] = {mist_coolant}; 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]; + cm.gm.mist_coolant = value[0]; if (cm.gm.mist_coolant) gpio_set_bit_on(MIST_COOLANT_BIT); else gpio_set_bit_off(MIST_COOLANT_BIT); @@ -1065,15 +1027,14 @@ static void _exec_mist_coolant_control(float *value, float *flag) { /// M8, M9 -stat_t cm_flood_coolant_control(uint8_t flood_coolant) { +void cm_flood_coolant_control(bool flood_coolant) { float value[AXES] = {flood_coolant}; mp_queue_command(_exec_flood_coolant_control, value, value); - return STAT_OK; } static void _exec_flood_coolant_control(float *value, float *flag) { - cm.gm.flood_coolant = (uint8_t)value[0]; + cm.gm.flood_coolant = value[0]; if (cm.gm.flood_coolant) gpio_set_bit_on(FLOOD_COOLANT_BIT); else { @@ -1090,70 +1051,59 @@ static void _exec_flood_coolant_control(float *value, float *flag) { */ /// M48, M49 -stat_t cm_override_enables(uint8_t flag) { - cm.gmx.feed_rate_override_enable = flag; - cm.gmx.traverse_override_enable = flag; - cm.gmx.spindle_override_enable = flag; - return STAT_OK; +void cm_override_enables(bool flag) { + cm.gm.feed_rate_override_enable = flag; + cm.gm.traverse_override_enable = flag; + cm.gm.spindle_override_enable = flag; } /// M50 -stat_t cm_feed_rate_override_enable(uint8_t flag) { +void cm_feed_rate_override_enable(bool flag) { if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter)) - cm.gmx.feed_rate_override_enable = false; - else cm.gmx.feed_rate_override_enable = true; - - return STAT_OK; + cm.gm.feed_rate_override_enable = false; + else cm.gm.feed_rate_override_enable = true; } /// M50.1 -stat_t cm_feed_rate_override_factor(uint8_t flag) { - cm.gmx.feed_rate_override_enable = flag; - cm.gmx.feed_rate_override_factor = cm.gn.parameter; - return STAT_OK; +void cm_feed_rate_override_factor(bool flag) { + cm.gm.feed_rate_override_enable = flag; + cm.gm.feed_rate_override_factor = cm.gn.parameter; } /// M50.2 -stat_t cm_traverse_override_enable(uint8_t flag) { +void cm_traverse_override_enable(bool flag) { if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter)) - cm.gmx.traverse_override_enable = false; - else cm.gmx.traverse_override_enable = true; - - return STAT_OK; + cm.gm.traverse_override_enable = false; + else cm.gm.traverse_override_enable = true; } /// M51 -stat_t cm_traverse_override_factor(uint8_t flag) { - cm.gmx.traverse_override_enable = flag; - cm.gmx.traverse_override_factor = cm.gn.parameter; - return STAT_OK; +void cm_traverse_override_factor(bool flag) { + cm.gm.traverse_override_enable = flag; + cm.gm.traverse_override_factor = cm.gn.parameter; } /// M51.1 -stat_t cm_spindle_override_enable(uint8_t flag) { +void cm_spindle_override_enable(bool flag) { if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter)) - cm.gmx.spindle_override_enable = false; - else cm.gmx.spindle_override_enable = true; - - return STAT_OK; + cm.gm.spindle_override_enable = false; + else cm.gm.spindle_override_enable = true; } /// M50.1 -stat_t cm_spindle_override_factor(uint8_t flag) { - cm.gmx.spindle_override_enable = flag; - cm.gmx.spindle_override_factor = cm.gn.parameter; - - return STAT_OK; +void cm_spindle_override_factor(bool flag) { + cm.gm.spindle_override_enable = flag; + cm.gm.spindle_override_factor = cm.gn.parameter; } -void cm_message(char *message) {printf(message);} +void cm_message(const char *message) {printf(message);} /* Program Functions (4.3.10) @@ -1246,8 +1196,7 @@ stat_t cm_queue_flush() { mp_flush_planner(); // flush planner queue // Note: The following uses low-level mp calls for absolute position. - // It could also use cm_get_absolute_position(RUNTIME, axis); - for (uint8_t axis = AXIS_X; axis < AXES; axis++) + for (int axis = 0; axis < AXES; axis++) // set mm from mr cm_set_position(axis, mp_get_runtime_absolute_position(axis)); @@ -1301,13 +1250,13 @@ static void _exec_program_finalize(float *value, float *flag) { // 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_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); + cm_set_plane(GCODE_DEFAULT_PLANE); + cm_set_distance_mode(GCODE_DEFAULT_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); + cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); } } @@ -1353,31 +1302,8 @@ void cm_program_end() { /// return ASCII char for axis given the axis number -char cm_get_axis_char(const int8_t axis) { +char cm_get_axis_char(int8_t axis) { char axis_char[] = "XYZABC"; if (axis < 0 || axis > AXES) return ' '; return axis_char[axis]; } - - -/* Jerk functions - * - * 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 axis_jerk() functions expect the jerk in divided-by 1,000,000 form - */ - -/// returns jerk for an axis -float cm_get_axis_jerk(uint8_t axis) { - return cm.a[axis].jerk_max; -} - - -/// sets the jerk for an axis, including recirpcal and cached values -void cm_set_axis_jerk(uint8_t axis, float jerk) { - cm.a[axis].jerk_max = jerk; - cm.a[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); -} diff --git a/src/canonical_machine.h b/src/canonical_machine.h index 2120d7d..3035a66 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -33,252 +33,14 @@ #include "status.h" #include +#include -/// absolute pointer from canonical machine gm model -#define MODEL (GCodeState_t *)&cm.gm -/// relative to buffer *bf is currently pointing to -#define PLANNER (GCodeState_t *)&bf->gm -/// absolute pointer from runtime mm struct -#define RUNTIME (GCodeState_t *)&mr.gm -/// active model pointer is maintained by state management -#define ACTIVE_MODEL cm.am -#define _to_millimeters(a) (cm.gm.units_mode == INCHES ? (a) * MM_PER_INCH : a) +#define TO_MILLIMETERS(a) (cm.gm.units_mode == INCHES ? (a) * MM_PER_INCH : a) #define DISABLE_SOFT_LIMIT -1000000 -/* 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. - * - * 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. - * - * - gn is used by the gcode interpreter and is re-initialized for - * each gcode block.It accepts data in the new gcode block in the - * formats present in the block (pre-normalized forms). During - * initialization some state elements are necessarily restored - * from gm. - * - * - gf is used by the gcode parser interpreter to hold flags for any - * data that has changed in gn during the parse. cm.gf.target[] - * values are also used by the canonical machine during - * set_target(). - * - * - 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). - */ - -/// Gcode model state - used by model, planning and runtime -typedef struct GCodeState { - uint32_t linenum; // Gcode block line number - uint8_t motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81, - // G82, G83 G84, G85, G86, G87, G88, G89 - float target[AXES]; // XYZABC where the move should go - /// offset from the work coordinate system (for reporting only) - float work_offset[AXES]; - - float move_time; // optimal time for move given axis constraints - /// minimum time possible for move given axis constraints - float minimum_time; - /// F - normalized to millimeters/minute or in inverse time mode - float feed_rate; - - float spindle_speed; // in RPM - /// P - parameter used for dwell time in seconds, G10 coord select... - float parameter; - - uint8_t feed_rate_mode; // See cmFeedRateMode for settings - uint8_t select_plane; // G17,G18,G19 - values to set plane to - uint8_t units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21) - uint8_t coord_system; // G54-G59 - select coordinate system 1-9 - /// G53 TRUE = move using machine coordinates - this block only (G53) - uint8_t absolute_override; - uint8_t path_control; // G61... EXACT_PATH, EXACT_STOP, CONTINUOUS - /// G91 0=use absolute coords(G90), 1=incremental movement - uint8_t distance_mode; - /// G91.1 0=use absolute coords(G90), 1=incremental movement - uint8_t arc_distance_mode; - uint8_t tool; // M6 tool change - moves "tool_select" to "tool" - uint8_t tool_select; // T value - T sets this value - uint8_t mist_coolant; // TRUE = mist on (M7), FALSE = off (M9) - uint8_t flood_coolant; // TRUE = flood on (M8), FALSE = off (M9) - uint8_t spindle_mode; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4) -} GCodeState_t; - - -/// Gcode dynamic state extensions - used by model and arcs -typedef struct GCodeStateExtended { - /// handles G modal group 1 moves & non-modals - uint8_t next_action; - uint8_t program_flow; // used only by the gcode_parser - - float position[AXES]; // model position (not used in gn or gf) - float origin_offset[AXES]; // G92 offsets (not used in gn or gf) - float g28_position[AXES]; // stored machine position for G28 - float g30_position[AXES]; // stored machine position for G30 - - float feed_rate_override_factor; // 1.0000 x F feed rate. - float traverse_override_factor; // 1.0000 x traverse rate. - uint8_t feed_rate_override_enable; // TRUE = overrides enabled (M48), F=(M49) - uint8_t traverse_override_enable; // TRUE = traverse override enabled - uint8_t l_word; // L word - used by G10s - - uint8_t origin_offset_enable; // G92 offsets enabled/disabled. - uint8_t block_delete_switch; // true enables block deletes (true) - - float spindle_override_factor; // 1.0000 x S spindle speed. - uint8_t spindle_override_enable; // TRUE = override enabled - - // unimplemented gcode parameters - // float cutter_radius; // D - cutter radius compensation (0 is off) - // float cutter_length; // H - cutter length compensation (0 is off) -} GCodeStateX_t; - - -/// Gcode model inputs - meaning depends on context -typedef struct GCodeInput { - /// handles G modal group 1 moves & non-modals - uint8_t next_action; - /// G0, G1, G2, G3, G38.2, G80, G81, G82, G83 G84, G85, G86, G87, G88 or G89 - uint8_t motion_mode; - uint8_t program_flow; // used only by the gcode_parser - uint32_t linenum; // N word or autoincrement in the model - - float target[AXES]; // XYZABC where the move should go - - float feed_rate; // F - normalized to millimeters/minute - float feed_rate_override_factor; // 1.0000 x F feed rate. - float traverse_override_factor; // 1.0000 x traverse rate. - uint8_t feed_rate_mode; // See cmFeedRateMode for settings - uint8_t feed_rate_override_enable; // TRUE = overrides enabled (M48), F=(M49) - uint8_t traverse_override_enable; // TRUE = traverse override enabled - uint8_t override_enables; // feed and spindle enable (GN/GF only) - uint8_t l_word; // L word - used by G10s - - uint8_t select_plane; // G17,G18,G19 - values to set plane to - uint8_t units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21) - uint8_t coord_system; // G54-G59 - select coordinate system 1-9 - uint8_t absolute_override; // G53 TRUE = move in machine coordinates - uint8_t origin_offset_mode; // G92 TRUE = in origin offset mode - uint8_t path_control; // G61 EXACT_PATH, EXACT_STOP, CONTINUOUS - uint8_t distance_mode; // G91 0=absolute(G90), 1=incremental - uint8_t arc_distance_mode; // G91.1 0=absolute(G90), 1=incremental - - uint8_t tool; // Tool after T and M6 - uint8_t tool_select; // T value - T sets this value - uint8_t tool_change; // M6 tool change flag - uint8_t mist_coolant; // TRUE = mist on (M7), FALSE = off (M9) - uint8_t flood_coolant; // TRUE = flood on (M8), FALSE = off (M9) - - uint8_t spindle_mode; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4) - float spindle_speed; // in RPM - float spindle_override_factor; // 1.0000 x S spindle speed. - uint8_t spindle_override_enable; // TRUE = override enabled - - float parameter; // P - dwell time in sec, G10 coord select - float arc_radius; // R - radius value in arc radius mode - float arc_offset[3]; // IJK - used by arc commands - - // unimplemented gcode parameters - // float cutter_radius; // D - cutter radius compensation (0 is off) - // float cutter_length; // H - cutter length compensation (0 is off) -} GCodeInput_t; - - -typedef struct cmAxis { - uint8_t axis_mode; // see tgAxisMode in gcode.h - float feedrate_max; // max velocity in mm/min or deg/min - float velocity_max; // max velocity in mm/min or deg/min - float travel_max; // max work envelope for soft limits - float travel_min; // min work envelope for soft limits - float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million - float jerk_homing; // homing jerk (Jh) in mm/min^3 divided by 1 million - float recip_jerk; // reciprocal of current jerk value - with million - float junction_dev; // aka cornering delta - float radius; // radius in mm for rotary axis modes - float search_velocity; // homing search velocity - float latch_velocity; // homing latch velocity - float latch_backoff; // backoff from switches prior to homing latch movement - float zero_backoff; // backoff from switches for machine zero -} cfgAxis_t; - - -typedef struct cmSingleton { // struct to manage cm globals and cycles - // Public - // system group settings - float junction_acceleration; // max cornering centripetal acceleration - float chordal_tolerance; // arc chordal accuracy setting in mm - uint8_t soft_limit_enable; - - // hidden system settings - float min_segment_len; // line drawing resolution in mm - float arc_segment_len; // arc drawing resolution in mm - float estd_segment_usec; // approximate segment time in msec - - // gcode power-on default settings - defaults are not the same as the gm state - uint8_t coord_system; // G10 active coordinate system default - uint8_t 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 - // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 - float offset[COORDS + 1][AXES]; - - // settings for axes X,Y,Z,A B,C - cfgAxis_t a[AXES]; - - // Private - // stat: combination of states for display purposes - uint8_t combined_state; - // macs: machine/cycle/motion is the actual machine state - uint8_t machine_state; - uint8_t cycle_state; // cycs - uint8_t motion_state; // momo - uint8_t hold_state; // hold: feedhold sub-state machine - uint8_t 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 feedhold_requested; // feedhold character has been received - /// queue flush character has been received - uint8_t queue_flush_requested; - /// cycle start character has been received (flag to end feedhold) - uint8_t cycle_start_requested; - struct GCodeState *am; // active model - - // Model states - GCodeState_t gm; // core gcode model state - GCodeStateX_t gmx; // extended gcode model state - GCodeInput_t gn; // gcode input values - transient - GCodeInput_t gf; // gcode input flags - transient -} cmSingleton_t; - - -extern cmSingleton_t cm; // canonical machine controller singleton - /* Machine state model * @@ -370,7 +132,7 @@ typedef enum { // applies to cm.homing_state typedef enum { // applies to cm.probe_state PROBE_FAILED, // probe reached endpoint without triggering PROBE_SUCCEEDED, // probe was triggered, cm.probe_results has position - PROBE_WAITING // probe is waiting to be started + PROBE_WAITING, // probe is waiting to be started } cmProbeState_t; @@ -378,6 +140,7 @@ typedef enum { // applies to cm.probe_state * used by the current block, and may carry non-modal commands, whereas * MotionMode persists across blocks (as G modal group 1) */ + /// these are in order to optimized CASE statement typedef enum { NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes) @@ -433,7 +196,7 @@ typedef enum { // Used for detecting gcode errors. See NIST section 3.4 MODAL_GROUP_M6, // {M6} tool change MODAL_GROUP_M7, // {M3,M4,M5} spindle turning MODAL_GROUP_M8, // {M7,M8,M9} coolant - MODAL_GROUP_M9 // {M48,M49} speed/feed override switches + MODAL_GROUP_M9, // {M48,M49} speed/feed override switches } cmModalGroup_t; #define MODAL_GROUP_COUNT (MODAL_GROUP_M9 + 1) @@ -504,12 +267,12 @@ typedef enum { } cmProgramFlow_t; -/// spindle state settings (See hardware.h for bit settings) +/// spindle state settings typedef enum { SPINDLE_OFF, SPINDLE_CW, SPINDLE_CCW -} cmSpindleState_t; +} cmSpindleMode_t; /// mist and flood coolant states @@ -537,102 +300,189 @@ typedef enum { AXIS_MODE_MAX } cmAxisMode_t; // ordering must be preserved. -#define AXIS_MODE_MAX_LINEAR AXIS_INHIBITED -#define AXIS_MODE_MAX_ROTARY AXIS_RADIUS - -// Coordinate offsets -#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_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 0 -#define G56_Y_OFFSET 0 -#define G56_Z_OFFSET 0 -#define G56_A_OFFSET 0 -#define G56_B_OFFSET 0 -#define G56_C_OFFSET 0 - -#define G57_X_OFFSET 0 -#define G57_Y_OFFSET 0 -#define G57_Z_OFFSET 0 -#define G57_A_OFFSET 0 -#define G57_B_OFFSET 0 -#define G57_C_OFFSET 0 - -#define G58_X_OFFSET 0 -#define G58_Y_OFFSET 0 -#define G58_Z_OFFSET 0 -#define G58_A_OFFSET 0 -#define G58_B_OFFSET 0 -#define G58_C_OFFSET 0 - -#define G59_X_OFFSET 0 -#define G59_Y_OFFSET 0 -#define G59_Z_OFFSET 0 -#define G59_A_OFFSET 0 -#define G59_B_OFFSET 0 -#define G59_C_OFFSET 0 + +/* 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. + * + * - gn is used by the gcode interpreter and is re-initialized for + * each gcode block.It accepts data in the new gcode block in the + * formats present in the block (pre-normalized forms). During + * initialization some state elements are necessarily restored + * from gm. + * + * - gf is used by the gcode parser interpreter to hold flags for any + * data that has changed in gn during the parse. cm.gf.target[] + * values are also used by the canonical machine during + * set_target(). + */ + + +typedef struct { + float target[AXES]; // XYZABC where the move should go + float work_offset[AXES]; // offset from work coordinate system + float move_time; // optimal time for move given axis constraints +} MoveState_t; + + +/// Gcode model state +typedef struct GCodeState { + uint32_t linenum; // Gcode block line number + + uint8_t tool; // Tool after T and M6 + uint8_t tool_select; // T - sets this value + + float feed_rate; // F - in mm/min or inverse time mode + cmFeedRateMode_t feed_rate_mode; + float feed_rate_override_factor; // 1.0000 x F feed rate. + bool feed_rate_override_enable; // M48, M49 + float traverse_override_factor; // 1.0000 x traverse rate + bool traverse_override_enable; + + float spindle_speed; // in RPM + cmSpindleMode_t spindle_mode; + float spindle_override_factor; // 1.0000 x S spindle speed + bool spindle_override_enable; // true = override enabled + + cmMotionMode_t motion_mode; // Group 1 modal motion + cmCanonicalPlane_t select_plane; // G17, G18, G19 + cmUnitsMode_t units_mode; // G20, G21 + cmCoordSystem_t coord_system; // G54-G59 - select coordinate system 1-9 + bool absolute_override; // G53 true = move in machine coordinates + cmPathControlMode_t path_control; // G61 + cmDistanceMode_t distance_mode; // G91 + cmDistanceMode_t arc_distance_mode; // G91.1 + + bool mist_coolant; // true = mist on (M7), false = off (M9) + bool flood_coolant; // true = flood on (M8), false = off (M9) + + cmNextAction_t next_action; // handles G group 1 moves & non-modals + cmProgramFlow_t program_flow; // used only by the gcode_parser + + // unimplemented gcode parameters + // float cutter_radius; // D - cutter radius compensation (0 is off) + // float cutter_length; // H - cutter length compensation (0 is off) + + // Used for input only + float target[AXES]; // XYZABC where the move should go + bool override_enables; // feed and spindle enable + bool tool_change; // M6 tool change flag + + float parameter; // P - dwell time in sec, G10 coord select + + float arc_radius; // R - radius value in arc radius mode + float arc_offset[3]; // IJK - used by arc commands +} GCodeState_t; + + +typedef struct cmAxis { + cmAxisMode_t axis_mode; + float feedrate_max; // max velocity in mm/min or deg/min + float velocity_max; // max velocity in mm/min or deg/min + float travel_max; // max work envelope for soft limits + float travel_min; // min work envelope for soft limits + float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million + float jerk_homing; // homing jerk (Jh) in mm/min^3 divided by 1 million + float recip_jerk; // reciprocal of current jerk value - with million + float junction_dev; // aka cornering delta + float radius; // radius in mm for rotary axis modes + float search_velocity; // homing search velocity + float latch_velocity; // homing latch velocity + float latch_backoff; // backoff from switches prior to homing latch movement + float zero_backoff; // backoff from switches for machine zero +} AxisConfig_t; + + +typedef struct cmSingleton { // struct to manage cm globals and cycles + // coordinate systems and offsets absolute (G53) + G54,G55,G56,G57,G58,G59 + float offset[COORDS + 1][AXES]; + float origin_offset[AXES]; // G92 offsets + bool origin_offset_enable; // G92 offsets enabled/disabled + + float position[AXES]; // model position (not used in gn or gf) + float g28_position[AXES]; // stored machine position for G28 + float g30_position[AXES]; // stored machine position for G30 + + // settings for axes X,Y,Z,A B,C + AxisConfig_t a[AXES]; + + cmCombinedState_t combined_state; // combination of states for display + cmMachineState_t machine_state; + cmCycleState_t cycle_state; + cmMotionState_t motion_state; + cmFeedholdState_t hold_state; // hold: feedhold sub-state machine + cmHomingState_t homing_state; // home: homing cycle sub-state machine + bool homed[AXES]; // individual axis homing flags + + cmProbeState_t probe_state; + float probe_results[AXES]; // probing results + + bool feedhold_requested; // feedhold character received + bool queue_flush_requested; // queue flush character received + bool cycle_start_requested; // cycle start character received + + // Model states + MoveState_t ms; + GCodeState_t gm; // core gcode model state + GCodeState_t gn; // gcode input values + GCodeState_t gf; // gcode input flags +} cmSingleton_t; + + +extern cmSingleton_t cm; // canonical machine controller singleton // Model state getters and setters -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(); -void cm_set_motion_state(uint8_t motion_state); +uint32_t cm_get_linenum(); +cmCombinedState_t cm_get_combined_state(); +cmMachineState_t cm_get_machine_state(); +cmCycleState_t cm_get_cycle_state(); +cmMotionState_t cm_get_motion_state(); +cmFeedholdState_t cm_get_hold_state(); +cmHomingState_t cm_get_homing_state(); +cmMotionMode_t cm_get_motion_mode(); +cmCoordSystem_t cm_get_coord_system(); +cmUnitsMode_t cm_get_units_mode(); +cmCanonicalPlane_t cm_get_select_plane(); +cmPathControlMode_t cm_get_path_control(); +cmDistanceMode_t cm_get_distance_mode(); +cmFeedRateMode_t cm_get_feed_rate_mode(); +uint8_t cm_get_tool(); +cmSpindleMode_t cm_get_spindle_mode(); +bool cm_get_runtime_busy(); +float cm_get_feed_rate(); + +void cm_set_motion_state(cmMotionState_t motion_state); +void cm_set_motion_mode(cmMotionMode_t motion_mode); +void cm_set_spindle_mode(cmSpindleMode_t spindle_mode); +void cm_set_spindle_speed_parameter(float speed); +void cm_set_tool_number(uint8_t tool); +void cm_set_absolute_override(bool absolute_override); +void cm_set_model_linenum(uint32_t linenum); + float cm_get_axis_jerk(uint8_t axis); void cm_set_axis_jerk(uint8_t axis, float jerk); -uint32_t cm_get_linenum(GCodeState_t *gcode_state); -uint8_t cm_get_motion_mode(GCodeState_t *gcode_state); -uint8_t cm_get_coord_system(GCodeState_t *gcode_state); -uint8_t cm_get_units_mode(GCodeState_t *gcode_state); -uint8_t cm_get_select_plane(GCodeState_t *gcode_state); -uint8_t cm_get_path_control(GCodeState_t *gcode_state); -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(); -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); -void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode); -void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed); -void cm_set_tool_number(GCodeState_t *gcode_state, uint8_t tool); -void cm_set_absolute_override(GCodeState_t *gcode_state, - uint8_t absolute_override); -void cm_set_model_linenum(uint32_t linenum); - // Coordinate systems and offsets float cm_get_active_coord_offset(uint8_t axis); -float cm_get_work_offset(GCodeState_t *gcode_state, uint8_t axis); -void cm_set_work_offsets(GCodeState_t *gcode_state); -float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t axis); -float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis); +float cm_get_work_offset(uint8_t axis); +void cm_set_work_offsets(); +float cm_get_absolute_position(uint8_t axis); +float cm_get_work_position(uint8_t axis); // Critical helpers +void cm_calc_move_time(const float axis_length[], const float axis_square[]); 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] +// Canonical machining functions defined by NIST [organized by NIST Gcode doc] // Initialization and termination (4.3.2) void canonical_machine_init(); @@ -643,32 +493,32 @@ stat_t cm_soft_alarm(stat_t status); stat_t cm_clear(); // Representation (4.3.3) -stat_t cm_select_plane(uint8_t plane); -stat_t cm_set_units_mode(uint8_t mode); -stat_t cm_set_distance_mode(uint8_t mode); -stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]); +void cm_set_plane(cmCanonicalPlane_t plane); +void cm_set_units_mode(cmUnitsMode_t mode); +void cm_set_distance_mode(cmDistanceMode_t mode); +void cm_set_coord_offsets(cmCoordSystem_t coord_system, float offset[], + float flag[]); -void cm_set_position(uint8_t axis, float position); -stat_t cm_set_absolute_origin(float origin[], float flag[]); -void cm_set_axis_origin(uint8_t axis, const float position); +void cm_set_position(int axis, float position); +void cm_set_absolute_origin(float origin[], float flag[]); -stat_t cm_set_coord_system(uint8_t coord_system); -stat_t cm_set_origin_offsets(float offset[], float flag[]); -stat_t cm_reset_origin_offsets(); -stat_t cm_suspend_origin_offsets(); -stat_t cm_resume_origin_offsets(); +void cm_set_coord_system(cmCoordSystem_t coord_system); +void cm_set_origin_offsets(float offset[], float flag[]); +void cm_reset_origin_offsets(); +void cm_suspend_origin_offsets(); +void cm_resume_origin_offsets(); // Free Space Motion (4.3.4) stat_t cm_straight_traverse(float target[], float flags[]); -stat_t cm_set_g28_position(); +void cm_set_g28_position(); stat_t cm_goto_g28_position(float target[], float flags[]); -stat_t cm_set_g30_position(); +void cm_set_g30_position(); stat_t cm_goto_g30_position(float target[], float flags[]); // Machining Attributes (4.3.5) -stat_t cm_set_feed_rate(float feed_rate); -stat_t cm_set_feed_rate_mode(uint8_t mode); -stat_t cm_set_path_control(uint8_t mode); +void cm_set_feed_rate(float feed_rate); +void cm_set_feed_rate_mode(cmFeedRateMode_t mode); +void cm_set_path_control(cmPathControlMode_t mode); // Machining Functions (4.3.6) stat_t cm_straight_feed(float target[], float flags[]); @@ -677,26 +527,25 @@ stat_t cm_arc_feed(float target[], float flags[], float radius, uint8_t motion_mode); stat_t cm_dwell(float seconds); -// Spindle Functions (4.3.7) -// see spindle.h for spindle definitions - which would go right here +// Spindle Functions (4.3.7) see spindle.h // Tool Functions (4.3.8) -stat_t cm_select_tool(uint8_t tool); -stat_t cm_change_tool(uint8_t tool); +void cm_select_tool(uint8_t tool); +void cm_change_tool(uint8_t tool); // Miscellaneous Functions (4.3.9) -stat_t cm_mist_coolant_control(uint8_t mist_coolant); -stat_t cm_flood_coolant_control(uint8_t flood_coolant); +void cm_mist_coolant_control(bool mist_coolant); +void cm_flood_coolant_control(bool flood_coolant); -stat_t cm_override_enables(uint8_t flag); -stat_t cm_feed_rate_override_enable(uint8_t flag); -stat_t cm_feed_rate_override_factor(uint8_t flag); -stat_t cm_traverse_override_enable(uint8_t flag); -stat_t cm_traverse_override_factor(uint8_t flag); -stat_t cm_spindle_override_enable(uint8_t flag); -stat_t cm_spindle_override_factor(uint8_t flag); +void cm_override_enables(bool flag); +void cm_feed_rate_override_enable(bool flag); +void cm_feed_rate_override_factor(bool flag); +void cm_traverse_override_enable(bool flag); +void cm_traverse_override_factor(bool flag); +void cm_spindle_override_enable(bool flag); +void cm_spindle_override_factor(bool flag); -void cm_message(char *message); +void cm_message(const char *message); // Program Functions (4.3.10) void cm_request_feedhold(); @@ -713,9 +562,8 @@ void cm_program_stop(); void cm_optional_program_stop(); void cm_program_end(); -char cm_get_axis_char(const int8_t axis); - // Cycles +char cm_get_axis_char(int8_t axis); // Homing cycles stat_t cm_homing_cycle_start(); diff --git a/src/config.h b/src/config.h index c8ee3fa..3140681 100644 --- a/src/config.h +++ b/src/config.h @@ -117,7 +117,7 @@ typedef enum { // Machine settings #define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing -#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on +#define SOFT_LIMIT_ENABLE false #define JERK_MAX 50 // yes, that's km/min^3 #define JUNCTION_DEVIATION 0.05 // default value, in mm #define JUNCTION_ACCELERATION 100000 // centripetal corner acceleration @@ -373,3 +373,46 @@ typedef enum { #define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius #define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies #define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test + +// Coordinate offsets +#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_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 0 +#define G56_Y_OFFSET 0 +#define G56_Z_OFFSET 0 +#define G56_A_OFFSET 0 +#define G56_B_OFFSET 0 +#define G56_C_OFFSET 0 + +#define G57_X_OFFSET 0 +#define G57_Y_OFFSET 0 +#define G57_Z_OFFSET 0 +#define G57_A_OFFSET 0 +#define G57_B_OFFSET 0 +#define G57_C_OFFSET 0 + +#define G58_X_OFFSET 0 +#define G58_Y_OFFSET 0 +#define G58_Z_OFFSET 0 +#define G58_A_OFFSET 0 +#define G58_B_OFFSET 0 +#define G58_C_OFFSET 0 + +#define G59_X_OFFSET 0 +#define G59_Y_OFFSET 0 +#define G59_Z_OFFSET 0 +#define G59_A_OFFSET 0 +#define G59_B_OFFSET 0 +#define G59_C_OFFSET 0 diff --git a/src/cycle_homing.c b/src/cycle_homing.c index 1298902..8f35af1 100644 --- a/src/cycle_homing.c +++ b/src/cycle_homing.c @@ -146,8 +146,8 @@ static int8_t _get_next_axis(int8_t axis); * * 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 cycle to be done. Otherwise there is a nasty race condition + * 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. */ @@ -156,11 +156,11 @@ static int8_t _get_next_axis(int8_t axis); /// G28.2 homing cycle using limit switches stat_t cm_homing_cycle_start() { // save relevant non-axis parameters from Gcode model - hm.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); - 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); + hm.saved_units_mode = cm_get_units_mode(&cm.gm); + hm.saved_coord_system = cm_get_coord_system(&cm.gm); + hm.saved_distance_mode = cm_get_distance_mode(&cm.gm); + hm.saved_feed_rate_mode = cm_get_feed_rate_mode(&cm.gm); + hm.saved_feed_rate = cm_get_feed_rate(&cm.gm); // set working values cm_set_units_mode(MILLIMETERS); @@ -329,7 +329,7 @@ static stat_t _homing_axis_set_zero(int8_t axis) { 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_position(axis, mp_get_runtime_work_position(axis)); cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value @@ -386,7 +386,7 @@ static stat_t _homing_finalize_exit(int8_t axis) { 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_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); cm_cycle_end(); cm.cycle_state = CYCLE_OFF; diff --git a/src/cycle_probing.c b/src/cycle_probing.c index 6da3a99..8f8165d 100644 --- a/src/cycle_probing.c +++ b/src/cycle_probing.c @@ -137,10 +137,9 @@ uint8_t cm_probe_callback() { * 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 + * 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. @@ -152,7 +151,7 @@ static uint8_t _probing_init() { for (uint8_t axis = 0; axis < AXES; axis++) { pb.saved_jerk[axis] = cm_get_axis_jerk(axis); // save the max jerk value cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use homing jerk for probe - pb.start_position[axis] = cm_get_absolute_position(ACTIVE_MODEL, axis); + pb.start_position[axis] = cm_get_absolute_position(axis); } // error if the probe target is too close to the current position @@ -183,8 +182,8 @@ static uint8_t _probing_init() { switch_init(); // probe in absolute machine coords - pb.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); - pb.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); + pb.saved_coord_system = cm_get_coord_system(&cm.gm); + pb.saved_distance_mode = cm_get_distance_mode(&cm.gm); cm_set_distance_mode(ABSOLUTE_MODE); cm_set_coord_system(ABSOLUTE_COORDS); @@ -214,7 +213,7 @@ static stat_t _probing_finish() { cm_set_position(axis, mp_get_runtime_work_position(axis)); // store the probe results - cm.probe_results[axis] = cm_get_absolute_position(ACTIVE_MODEL, axis); + cm.probe_results[axis] = cm_get_absolute_position(axis); } return _set_pb_func(_probing_finalize_exit); @@ -238,7 +237,7 @@ static void _probe_restore_settings() { cm_set_distance_mode(pb.saved_distance_mode); // update the model with actual position - cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); + cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); cm_cycle_end(); cm.cycle_state = CYCLE_OFF; } diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 6afd7a4..47d75cb 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -56,7 +56,7 @@ 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;} -#define EXEC_FUNC(f, v) if ((uint8_t)cm.gf.v) {status = f(cm.gn.v);} +#define EXEC_FUNC(f, v) if ((uint8_t)cm.gf.v) f(cm.gn.v) /// Parse a block (line) of gcode @@ -249,10 +249,10 @@ static stat_t _parse_gcode_block(char *buf) { // 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 + memset(&cm.gf, 0, sizeof(GCodeState_t)); // clear all next-state flags + memset(&cm.gn, 0, sizeof(GCodeState_t)); // clear all next-state values // get motion mode from previous block - cm.gn.motion_mode = cm_get_motion_mode(MODEL); + cm.gn.motion_mode = cm_get_motion_mode(); // extract commands and parameters while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { @@ -392,15 +392,15 @@ static stat_t _parse_gcode_block(char *buf) { 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 '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 + case 'N': SET_NON_MODAL(linenum, (uint32_t)value); // line number + case 'L': break; // not used for anything case 0: break; default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } @@ -478,7 +478,7 @@ static stat_t _execute_gcode_block() { // return if error, otherwise complete the block ritorno(cm_dwell(cm.gn.parameter)); - EXEC_FUNC(cm_select_plane, select_plane); + EXEC_FUNC(cm_set_plane, select_plane); EXEC_FUNC(cm_set_units_mode, units_mode); //--> cutter radius compensation goes here //--> cutter length compensation goes here @@ -489,13 +489,13 @@ static stat_t _execute_gcode_block() { switch (cm.gn.next_action) { case NEXT_ACTION_SET_G28_POSITION: // G28.1 - status = cm_set_g28_position(); + cm_set_g28_position(); break; case NEXT_ACTION_GOTO_G28_POSITION: // G28 status = cm_goto_g28_position(cm.gn.target, cm.gf.target); break; case NEXT_ACTION_SET_G30_POSITION: // G30.1 - status = cm_set_g30_position(); + cm_set_g30_position(); break; case NEXT_ACTION_GOTO_G30_POSITION: // G30 status = cm_goto_g30_position(cm.gn.target, cm.gf.target); @@ -504,7 +504,7 @@ static stat_t _execute_gcode_block() { status = cm_homing_cycle_start(); break; case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3 - status = cm_set_absolute_origin(cm.gn.target, cm.gf.target); + cm_set_absolute_origin(cm.gn.target, cm.gf.target); break; case NEXT_ACTION_HOMING_NO_SET: // G28.4 status = cm_homing_cycle_start_no_set(); @@ -513,24 +513,26 @@ static stat_t _execute_gcode_block() { status = cm_straight_probe(cm.gn.target, cm.gf.target); break; case NEXT_ACTION_SET_COORD_DATA: - status = cm_set_coord_offsets(cm.gn.parameter, cm.gn.target, cm.gf.target); + 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); + cm_set_origin_offsets(cm.gn.target, cm.gf.target); break; case NEXT_ACTION_RESET_ORIGIN_OFFSETS: - status = cm_reset_origin_offsets(); + cm_reset_origin_offsets(); break; case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: - status = cm_suspend_origin_offsets(); + cm_suspend_origin_offsets(); break; case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: - status = cm_resume_origin_offsets(); + cm_resume_origin_offsets(); break; + case NEXT_ACTION_DWELL: break; // Handled above case NEXT_ACTION_DEFAULT: // apply override setting to gm struct - cm_set_absolute_override(MODEL, cm.gn.absolute_override); + cm_set_absolute_override(cm.gn.absolute_override); + switch (cm.gn.motion_mode) { case MOTION_MODE_CANCEL_MOTION_MODE: cm.gm.motion_mode = cm.gn.motion_mode; @@ -547,10 +549,11 @@ static stat_t _execute_gcode_block() { cm.gn.arc_offset[1], cm.gn.arc_offset[2], cm.gn.arc_radius, cm.gn.motion_mode); break; + default: break; // Should not get here } } // un-set absolute override once the move is planned - cm_set_absolute_override(MODEL, false); + cm_set_absolute_override(false); // do the program stops and ends : M0, M1, M2, M30, M60 if (cm.gf.program_flow) { diff --git a/src/plan/arc.c b/src/plan/arc.c index 21e2e98..477e690 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -74,7 +74,7 @@ typedef struct arArcSingleton { // persistent planner and runtime variables 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; // state struct is passed for each arc segment. + MoveState_t ms; } arc_t; arc_t arc = {}; @@ -192,8 +192,8 @@ static void _estimate_arc_time() { */ 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]; + float x = cm.ms.target[arc.plane_axis_0] - cm.position[arc.plane_axis_0]; + float y = cm.ms.target[arc.plane_axis_1] - cm.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 @@ -263,9 +263,9 @@ static stat_t _compute_arc() { // (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." // Compute end radius from the center of circle (offsets) to target endpoint - float end_0 = arc.gm.target[arc.plane_axis_0] - + float end_0 = arc.ms.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0]; - float end_1 = arc.gm.target[arc.plane_axis_1] - + float end_1 = arc.ms.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1]; // end radius - start radius float err = fabs(hypotf(end_0, end_1) - arc.radius); @@ -321,7 +321,7 @@ static stat_t _compute_arc() { // 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.ms.target[arc.linear_axis] - arc.position[arc.linear_axis]; arc.planar_travel = arc.angular_travel * arc.radius; // hypot is insensitive to +/- signs arc.length = hypotf(arc.planar_travel, arc.linear_travel); @@ -330,9 +330,9 @@ static stat_t _compute_arc() { // 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; + arc.length / sqrt(4 * CHORDAL_TOLERANCE * + (2 * arc.radius - CHORDAL_TOLERANCE)); + float arc_segments_for_minimum_distance = arc.length / ARC_SEGMENT_LENGTH; float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; @@ -343,14 +343,14 @@ static stat_t _compute_arc() { arc.arc_segments = max(arc.arc_segments, 1); // at least 1 arc_segment // gcode state struct gets arc_segment_time, not arc time - arc.gm.move_time = arc.arc_time / arc.arc_segments; + arc.ms.move_time = arc.arc_time / arc.arc_segments; arc.arc_segment_count = (int32_t)arc.arc_segments; arc.arc_segment_theta = arc.angular_travel / arc.arc_segments; arc.arc_segment_linear_travel = arc.linear_travel / arc.arc_segments; arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; // initialize the linear target - arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; + arc.ms.target[arc.linear_axis] = arc.position[arc.linear_axis]; return STAT_OK; } @@ -431,23 +431,23 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints cm_set_model_target(target, flags); // in radius mode it's an error for start == end - if (radius_f && 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])) + if (radius_f && fp_EQ(cm.position[AXIS_X], cm.ms.target[AXIS_X]) && + fp_EQ(cm.position[AXIS_Y], cm.ms.target[AXIS_Y]) && + fp_EQ(cm.position[AXIS_Z], cm.ms.target[AXIS_Z])) 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); // resolved offsets to gm - memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // context to arc singleton + memcpy(&arc.ms, &cm.ms, sizeof(MoveState_t));// context to arc singleton - copy_vector(arc.position, cm.gmx.position); // arc pos from gcode model + copy_vector(arc.position, cm.position); // arc pos 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); // offsets canonical form (mm) - arc.offset[1] = _to_millimeters(j); - arc.offset[2] = _to_millimeters(k); + arc.offset[0] = TO_MILLIMETERS(i); // offsets canonical form (mm) + arc.offset[1] = TO_MILLIMETERS(j); + arc.offset[2] = TO_MILLIMETERS(k); arc.rotations = floor(fabs(cm.gn.parameter)); // P must be positive integer @@ -482,11 +482,11 @@ stat_t cm_arc_callback() { 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 pos + arc.ms.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius; + arc.ms.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; + arc.ms.target[arc.linear_axis] += arc.arc_segment_linear_travel; + mp_aline(&arc.ms); // run the line + copy_vector(arc.position, arc.ms.target); // update arc current pos if (--arc.arc_segment_count > 0) return STAT_EAGAIN; diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 945fe79..0e321f5 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -117,7 +117,7 @@ typedef struct mpBuffer { // See Planning Velocity Notes float recip_jerk; // 1/Jm used for planning (computed & cached) float cbrt_jerk; // cube root of Jm (computed & cached) - GCodeState_t gm; // Gode model state, used by planner & runtime + MoveState_t ms; } mpBuf_t; diff --git a/src/plan/command.c b/src/plan/command.c index a2ef363..3d0f2bb 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -73,7 +73,7 @@ void mp_queue_command(cm_exec_t cm_exec, float *value, float *flag) { // Store values and flags in planner buffer for (int axis = 0; axis < AXES; axis++) { - bf->gm.target[axis] = value[axis]; + bf->ms.target[axis] = value[axis]; bf->unit[axis] = flag[axis]; // flag vector in unit } @@ -84,7 +84,7 @@ void mp_queue_command(cm_exec_t cm_exec, float *value, float *flag) { void mp_runtime_command(mpBuf_t *bf) { // Use values & flags stored in mp_queue_command() - bf->cm_func(bf->gm.target, bf->unit); + bf->cm_func(bf->ms.target, bf->unit); // Free buffer & perform cycle_end if planner is empty if (mp_free_run_buffer()) cm_cycle_end(); diff --git a/src/plan/dwell.c b/src/plan/dwell.c index f1f6e29..55953d4 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -39,7 +39,7 @@ /// Dwell execution static stat_t _exec_dwell(mpBuf_t *bf) { - st_prep_dwell(bf->gm.move_time); // in seconds + st_prep_dwell(bf->ms.move_time); // in seconds // free buffer & perform cycle_end if planner is empty if (mp_free_run_buffer()) cm_cycle_end(); @@ -56,7 +56,7 @@ stat_t mp_dwell(float seconds) { if (!bf) return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); bf->bf_func = _exec_dwell; // register callback to dwell start - bf->gm.move_time = seconds; // in seconds, not minutes + bf->ms.move_time = seconds; // in seconds, not minutes bf->move_state = MOVE_NEW; // must be final operation before exit diff --git a/src/plan/exec.c b/src/plan/exec.c index a35494c..66a2c66 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -147,7 +147,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // initialization to process the new incoming bf buffer (Gcode block) // copy in the gcode model state - memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t)); + memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t)); bf->replannable = false; // short lines have already been removed, look for an actual zero @@ -181,7 +181,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { 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 + copy_vector(mr.final_target, bf->ms.target); // save move final target // generate the waypoints for position correction at section ends for (uint8_t axis = 0; axis < AXES; axis++) { @@ -248,9 +248,9 @@ static stat_t _exec_aline_body() { 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.ms.move_time = mr.body_length / mr.cruise_velocity; + mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); + mr.segment_time = mr.ms.move_time / mr.segments; mr.segment_velocity = mr.cruise_velocity; mr.segment_count = (uint32_t)mr.segments; @@ -284,10 +284,10 @@ static stat_t _exec_aline_head() { mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2; // time for entire accel region - mr.gm.move_time = mr.head_length / mr.midpoint_velocity; + mr.ms.move_time = mr.head_length / mr.midpoint_velocity; // # of segments in *each half* - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); - mr.segment_time = mr.gm.move_time / (2 * mr.segments); + mr.segments = ceil(uSec(mr.ms.move_time) / (2 * NOM_SEGMENT_USEC)); + mr.segment_time = mr.ms.move_time / (2 * mr.segments); mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); mr.midpoint_acceleration = @@ -345,11 +345,11 @@ static stat_t _exec_aline_tail() { 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.ms.move_time = mr.tail_length / mr.midpoint_velocity; // # of segments in *each half* - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); + mr.segments = ceil(uSec(mr.ms.move_time) / (2 * NOM_SEGMENT_USEC)); // time to advance for each segment - mr.segment_time = mr.gm.move_time / (2 * mr.segments); + mr.segment_time = mr.ms.move_time / (2 * mr.segments); mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk); mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time; @@ -556,11 +556,11 @@ static stat_t _exec_aline_head() { } // Time for entire accel region - mr.gm.move_time = + mr.ms.move_time = 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); // # of segments for the section - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); - mr.segment_time = mr.gm.move_time / mr.segments; + mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); + mr.segment_time = mr.ms.move_time / mr.segments; _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); mr.segment_count = (uint32_t)mr.segments; @@ -645,12 +645,12 @@ static stat_t _exec_aline_tail() { if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move // len/avg. velocity - mr.gm.move_time = + mr.ms.move_time = 2 * mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); // # of segments for the section - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); + mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); // time to advance for each segment - mr.segment_time = mr.gm.move_time / mr.segments; + mr.segment_time = mr.ms.move_time / mr.segments; _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); mr.segment_count = (uint32_t)mr.segments; @@ -755,13 +755,13 @@ static stat_t _exec_aline_segment() { // 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]); + copy_vector(mr.ms.target, mr.waypoint[mr.section]); else { float segment_length = mr.segment_velocity * mr.segment_time; for (i = 0; i < AXES; i++) - mr.gm.target[i] = mr.position[i] + (mr.unit[i] * segment_length); + mr.ms.target[i] = mr.position[i] + (mr.unit[i] * segment_length); } // Convert target position to steps. Bucket-brigade the old target @@ -782,7 +782,7 @@ static stat_t _exec_aline_segment() { } // now determine the target steps - ik_kinematics(mr.gm.target, mr.target_steps); + ik_kinematics(mr.ms.target, mr.target_steps); // and compute the distances to be traveled for (i = 0; i < MOTORS; i++) @@ -791,7 +791,7 @@ static stat_t _exec_aline_segment() { // Call the stepper prep function ritorno(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); // update position from target - copy_vector(mr.position, mr.gm.target); + copy_vector(mr.position, mr.ms.target); #ifdef __JERK_EXEC // needed by jerk-based exec (ignored if running body) mr.elapsed_accel_time += mr.segment_accel_time; diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index 0b6458b..f14a566 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -133,7 +133,7 @@ stat_t mp_plan_hold_callback() { float braking_length; // distance to brake to zero from braking_velocity // examine and process mr buffer - mr_available_length = get_axis_vector_length(mr.target, mr.position); + mr_available_length = get_axis_vector_length(mr.final_target, mr.position); // compute next_segment velocity braking_velocity = _compute_next_segment_velocity(); diff --git a/src/plan/line.c b/src/plan/line.c index 5007e7f..cd00979 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -158,127 +158,12 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { 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); + float velocity = sqrt(radius * JUNCTION_ACCELERATION); return velocity; } -/* Compute optimal and minimum move times into the gcode_state - * - * "Minimum time" is the fastest the move can be performed given - * the velocity constraints on each participating axis - regardless - * of the feed rate requested. The minimum time is the time limited - * by the rate-limiting axis. The minimum time is needed to compute - * the optimal time and is recorded for possible feed override - * computation. - * - * "Optimal time" is either the time resulting from the requested - * feed rate or the minimum time if the requested feed rate is not - * achievable. Optimal times for traverses are always the minimum - * time. - * - * The gcode state must have targets set prior by having - * cm_set_target(). Axis modes are taken into account by this. - * - * The following times are compared and the longest is returned: - * - G93 inverse time (if G93 is active) - * - time for coordinated move at requested feed rate - * - time that the slowest axis would require for the move - * - * Sets the following variables in the gcode_state struct - * - move_time is set to optimal time - * - minimum_time is set to minimum time - * - * NIST RS274NGC_v3 Guidance - * - * The following is verbatim text from NIST RS274NGC_v3. As I - * interpret A for moves that combine both linear and rotational - * movement, the feed rate should apply to the XYZ movement, with - * the rotational axis (or axes) timed to start and end at the same - * time the linear move is performed. It is possible under this - * case for the rotational move to rate-limit the linear move. - * - * 2.1.2.5 Feed Rate - * - * The rate at which the controlled point or the axes move is - * nominally a steady rate which may be set by the user. In the - * Interpreter, the interpretation of the feed rate is as follows - * unless inverse time feed rate mode is being used in the - * RS274/NGC view (see Section 3.5.19). The canonical machining - * functions view of feed rate, as described in Section 4.3.5.1, - * has conditions under which the set feed rate is applied - * differently, but none of these is used in the Interpreter. - * - * A. For motion involving one or more of the X, Y, and Z axes - * (with or without simultaneous rotational axis motion), the - * feed rate means length units per minute along the programmed - * XYZ path, as if the rotational axes were not moving. - * - * B. For motion of one rotational axis with X, Y, and Z axes not - * moving, the feed rate means degrees per minute rotation of - * the rotational axis. - * - * C. For motion of two or three rotational axes with X, Y, and Z - * axes not moving, the rate is applied as follows. Let dA, dB, - * and dC be the angles in degrees through which the A, B, and - * C axes, respectively, must move. Let D = sqrt(dA^2 + dB^2 + - * dC^2). Conceptually, D is a measure of total angular motion, - * using the usual Euclidean metric. Let T be the amount of - * time required to move through D degrees at the current feed - * rate in degrees per minute. The rotational axes should be - * moved in coordinated linear motion so that the elapsed time - * from the start to the end of the motion is T plus any time - * required for acceleration or deceleration. - */ -static void _calc_move_times(GCodeState_t *gms, const float axis_length[], - const float axis_square[]) { - // gms = Gcode model state - float inv_time = 0; // inverse time if doing a feed in G93 mode - float xyz_time = 0; // linear coordinated move at requested feed - float abc_time = 0; // rotary coordinated move at requested feed - float max_time = 0; // time required for the rate-limiting axis - float tmp_time = 0; // used in computation - gms->minimum_time = 8675309; // arbitrarily large number - - // compute times for feed motion - if (gms->motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) { - if (gms->feed_rate_mode == INVERSE_TIME_MODE) { - // feed rate was un-inverted to minutes by cm_set_feed_rate() - inv_time = gms->feed_rate; - gms->feed_rate_mode = UNITS_PER_MINUTE_MODE; - - } else { - // compute length of linear move in millimeters. Feed rate is provided as - // mm/min - xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + - axis_square[AXIS_Z]) / gms->feed_rate; - - // if no linear axes, compute length of multi-axis rotary move in degrees. - // Feed rate is provided as degrees/min - if (fp_ZERO(xyz_time)) - abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + - axis_square[AXIS_C]) / gms->feed_rate; - } - } - - for (uint8_t axis = 0; axis < AXES; axis++) { - if (gms->motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE) - tmp_time = fabs(axis_length[axis]) / cm.a[axis].velocity_max; - - else // MOTION_MODE_STRAIGHT_FEED - tmp_time = fabs(axis_length[axis]) / cm.a[axis].feedrate_max; - - max_time = max(max_time, tmp_time); - - if (tmp_time > 0) // collect minimum time if this axis is not zero - gms->minimum_time = min(gms->minimum_time, tmp_time); - } - - gms->move_time = max4(inv_time, max_time, xyz_time, abc_time); -} - - /* Plan a line with acceleration / deceleration * * This function uses constant jerk motion equations to plan @@ -297,7 +182,7 @@ static void _calc_move_times(GCodeState_t *gms, const float axis_length[], * accumulate and get executed once the accumulated error exceeds * the minimums. */ -stat_t mp_aline(GCodeState_t *gm_in) { +stat_t mp_aline(MoveState_t *ms) { float exact_stop = 0; // preset this value OFF float junction_velocity; uint8_t mr_flag = false; @@ -308,7 +193,7 @@ stat_t mp_aline(GCodeState_t *gm_in) { float length_square = 0; for (uint8_t axis = 0; axis < AXES; axis++) { - axis_length[axis] = gm_in->target[axis] - mm.position[axis]; + axis_length[axis] = ms->target[axis] - mm.position[axis]; axis_square[axis] = square(axis_length[axis]); length_square += axis_square[axis]; } @@ -319,7 +204,7 @@ stat_t mp_aline(GCodeState_t *gm_in) { return STAT_OK; } - // If _calc_move_times() says the move will take less than the + // If cm_calc_move_time() 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 @@ -333,10 +218,10 @@ stat_t mp_aline(GCodeState_t *gm_in) { // (3) Previous block is not optimally planned. // Vi <= previous block's entry_velocity + delta_velocity - // Set move & minimum time in state - _calc_move_times(gm_in, axis_length, axis_square); + // Set move time in state + cm_calc_move_time(axis_length, axis_square); - if (gm_in->move_time < MIN_BLOCK_TIME) { + if (ms->move_time < MIN_BLOCK_TIME) { // Max velocity change for this move float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; float entry_velocity = 0; // pre-set as if no previous block @@ -360,8 +245,9 @@ stat_t mp_aline(GCodeState_t *gm_in) { // Register callback to exec function bf->bf_func = mp_exec_aline; bf->length = length; + // Copy model state into planner buffer - memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); + memcpy(&bf->ms, ms, sizeof(MoveState_t)); // Compute the unit vector and find the right jerk to use (combined // operations) To determine the jerk value to use for the block we @@ -462,12 +348,12 @@ stat_t mp_aline(GCodeState_t *gm_in) { // finish up the current block variables // exact stop cases already zeroed - if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { + if (cm_get_path_control() != PATH_EXACT_STOP) { bf->replannable = true; exact_stop = 8675309; // an arbitrarily large floating point number } - bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested + bf->cruise_vmax = bf->length / bf->ms.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); @@ -478,7 +364,7 @@ stat_t mp_aline(GCodeState_t *gm_in) { // Note: these next lines must remain in exact order. Position must update // before committing the buffer. mp_plan_block_list(bf, &mr_flag); // replan block list - copy_vector(mm.position, bf->gm.target); // set the planner position + copy_vector(mm.position, bf->ms.target); // set the planner position // commit current block (must follow the position update) mp_commit_write_buffer(MOVE_TYPE_ALINE); @@ -539,7 +425,7 @@ stat_t mp_aline(GCodeState_t *gm_in) { * 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->ms.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. diff --git a/src/plan/line.h b/src/plan/line.h index 11044ce..8093b27 100644 --- a/src/plan/line.h +++ b/src/plan/line.h @@ -31,4 +31,4 @@ void mp_set_planner_position(uint8_t axis, const float position); void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); -stat_t mp_aline(GCodeState_t *gm_in); +stat_t mp_aline(MoveState_t *ms); diff --git a/src/plan/planner.c b/src/plan/planner.c index 296bf6b..fa48ecc 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -106,7 +106,7 @@ void mp_flush_planner() { * * - mm.position - start and end position for planning * - mr.position - current position of runtime segment - * - mr.target - target position of runtime segment + * - mr.ms.target - target position of runtime segment * - mr.endpoint - final target position of runtime segment * * Note that position is set immediately when called and may not be @@ -157,14 +157,14 @@ float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];} /// Set offsets in the MR struct void mp_set_runtime_work_offset(float offset[]) { - copy_vector(mr.gm.work_offset, offset); + copy_vector(mr.ms.work_offset, offset); } /// Returns current axis position in work coordinates /// that were in effect at move planning time float mp_get_runtime_work_position(uint8_t axis) { - return mr.position[axis] - mr.gm.work_offset[axis]; + return mr.position[axis] - mr.ms.work_offset[axis]; } diff --git a/src/plan/planner.h b/src/plan/planner.h index 1114605..f8f4456 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -66,7 +66,7 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables /// unit vector for axis scaling & planning float unit[AXES]; /// final target for bf (used to correct rounding errors) - float target[AXES]; + float final_target[AXES]; /// current move position float position[AXES]; /// for Kahan summation in _exec_aline_segment() @@ -114,7 +114,7 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables #endif #endif // __JERK_EXEC - GCodeState_t gm; // gcode model state currently executing + MoveState_t ms; } mpMoveRuntimeSingleton_t; diff --git a/src/report.c b/src/report.c index ab28db8..7779630 100644 --- a/src/report.c +++ b/src/report.c @@ -27,7 +27,6 @@ #include "report.h" #include "config.h" -#include "canonical_machine.h" #include "usart.h" #include "rtc.h" #include "vars.h" @@ -70,7 +69,7 @@ stat_t report_callback() { float get_position(int index) { - return cm_get_absolute_position(0, index); + return mp_get_runtime_absolute_position(index); } diff --git a/src/spindle.c b/src/spindle.c index 332dbf5..7951bb2 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -66,9 +66,9 @@ static spindle_t spindle = { /// execute the spindle command (called from planner) static void _exec_spindle_control(float *value, float *flag) { - uint8_t spindle_mode = (uint8_t)value[0]; + uint8_t spindle_mode = value[0]; - cm_set_spindle_mode(MODEL, spindle_mode); + cm_set_spindle_mode(spindle_mode); if (spindle_mode == SPINDLE_CW) { gpio_set_bit_on(SPINDLE_BIT); @@ -81,13 +81,13 @@ static void _exec_spindle_control(float *value, float *flag) { } 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) ); + pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode)); } /// Spindle speed callback from planner queue static void _exec_spindle_speed(float *value, float *flag) { - cm_set_spindle_speed_parameter(MODEL, value[0]); + cm_set_spindle_speed_parameter(value[0]); // update spindle speed if we're running pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode)); } @@ -103,7 +103,7 @@ void cm_spindle_init() { /// return PWM phase (duty cycle) for dir and speed -float cm_get_spindle_pwm(uint8_t spindle_mode) { +float cm_get_spindle_pwm(cmSpindleMode_t spindle_mode) { float speed_lo = 0, speed_hi = 0, phase_lo = 0, phase_hi = 0; if (spindle_mode == SPINDLE_CW) { @@ -133,28 +133,20 @@ float cm_get_spindle_pwm(uint8_t spindle_mode) { /// queue the spindle command to the planner buffer -stat_t cm_spindle_control(uint8_t spindle_mode) { - float value[AXES] = {spindle_mode, 0, 0, 0, 0, 0}; - +void cm_spindle_control(cmSpindleMode_t spindle_mode) { + float value[AXES] = {spindle_mode}; mp_queue_command(_exec_spindle_control, value, value); - - return STAT_OK; } + /// Queue the S parameter to the planner buffer -stat_t cm_set_spindle_speed(float speed) { - float value[AXES] = { speed, 0,0,0,0,0 }; +void cm_set_spindle_speed(float speed) { + float value[AXES] = {speed}; mp_queue_command(_exec_spindle_speed, value, value); - return STAT_OK; -} - - -/// Execute the S command (called from the planner buffer) -void cm_exec_spindle_speed(float speed) { - cm_set_spindle_speed(speed); } +// TODO implement these float get_max_spin(int index) { return 0; } diff --git a/src/spindle.h b/src/spindle.h index 22bdc49..07a7bc5 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -28,13 +28,10 @@ #pragma once +#include "canonical_machine.h" -#include "status.h" void cm_spindle_init(); - -stat_t cm_set_spindle_speed(float speed); // S parameter -void cm_exec_spindle_speed(float speed); // callback for above -float cm_get_spindle_pwm(uint8_t spindle_mode); -stat_t cm_spindle_control(uint8_t spindle_mode); // M3, M4, M5 -void cm_exec_spindle_control(uint8_t spindle_mode); // callback for above +void cm_set_spindle_speed(float speed); // S parameter +float cm_get_spindle_pwm(cmSpindleMode_t spindle_mode); +void cm_spindle_control(cmSpindleMode_t spindle_mode); // M3, M4, M5 diff --git a/src/util.h b/src/util.h index 162ee25..c92205a 100644 --- a/src/util.h +++ b/src/util.h @@ -45,7 +45,7 @@ extern float vector[AXES]; // vector of axes for passing to subroutines #define clear_vector(a) memset(a, 0, sizeof(a)) -#define copy_vector(d,s) memcpy(d, s, sizeof(d)) +#define copy_vector(d, s) memcpy(d, s, sizeof(d)) float get_axis_vector_length(const float a[], const float b[]); uint8_t vector_equal(const float a[], const float b[]); -- 2.27.0