--- /dev/null
+#!/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)
* 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
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
// State
.gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE},
- .gmx = {.block_delete_switch = true},
.gn = {},
.gf = {},
};
// 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;
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);
}
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;
}
* 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
}
+/* 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);
}
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;
}
}
* 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;
target[axis] > cm.a[axis].travel_max)
return STAT_SOFT_LIMIT_EXCEEDED;
}
+#endif
return STAT_OK;
}
// 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
// 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.
* 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]);
}
// (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
}
* 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();
* 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
*/
/// 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;
}
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;
/// 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);
// 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);
}
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;
*/
/// 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;
}
// 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);
/// 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 {
*/
/// 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)
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));
// 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);
}
}
/// 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);
-}
#include "status.h"
#include <stdint.h>
+#include <stdbool.h>
-/// 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
*
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;
* 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)
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)
} 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
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();
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[]);
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();
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();
// 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
#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
*
* 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.
*/
/// 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);
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
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;
* 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.
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
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);
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);
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;
}
#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
// 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) {
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;
}
// 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
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);
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();
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;
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) {
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 = {};
*/
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
// (.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);
// 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);
// 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;
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;
}
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
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;
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;
// 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
}
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();
/// 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();
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
// 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
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++) {
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;
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 =
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;
}
// 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;
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;
// 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
}
// 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++)
// 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;
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();
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
* 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;
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];
}
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
// (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
// 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
// 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);
// 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);
* 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.
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);
*
* - 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
/// 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];
}
/// 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()
#endif
#endif // __JERK_EXEC
- GCodeState_t gm; // gcode model state currently executing
+ MoveState_t ms;
} mpMoveRuntimeSingleton_t;
#include "report.h"
#include "config.h"
-#include "canonical_machine.h"
#include "usart.h"
#include "rtc.h"
#include "vars.h"
float get_position(int index) {
- return cm_get_absolute_position(0, index);
+ return mp_get_runtime_absolute_position(index);
}
/// 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);
} 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));
}
/// 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) {
/// 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;
}
#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
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[]);
projects / bbctrl-firmware / commitdiff