From: Joseph Coffland Date: Sun, 11 Sep 2016 11:24:38 +0000 (-0700) Subject: Fully encapsulated mach X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=970bfb936c709e791587c2d458a038f42812aa61;p=bbctrl-firmware Fully encapsulated mach --- diff --git a/src/machine.c b/src/machine.c index a7dfda0..009d415 100644 --- a/src/machine.c +++ b/src/machine.c @@ -87,8 +87,20 @@ #define DISABLE_SOFT_LIMIT -1000000 +typedef struct { // struct to manage mach globals and cycles + float offset[COORDS + 1][AXES]; // coordinate systems & offsets G53-G59 + float origin_offset[AXES]; // G92 offsets + bool origin_offset_enable; // G92 offsets enabled / disabled -machine_t mach = { + float position[AXES]; // model position + float g28_position[AXES]; // stored machine position for G28 + float g30_position[AXES]; // stored machine position for G30 + + gcode_state_t gm; // core gcode model state +} machine_t; + + +static machine_t mach = { // Offsets .offset = { {}, // ABSOLUTE_COORDS @@ -108,7 +120,6 @@ machine_t mach = { // Machine State functions uint32_t mach_get_line() {return mach.gm.line;} -uint8_t mach_get_tool() {return mach.gm.tool;} float mach_get_feed_rate() {return mach.gm.feed_rate;} feed_mode_t mach_get_feed_mode() {return mach.gm.feed_mode;} @@ -325,12 +336,20 @@ void mach_update_work_offsets() { } +const float *mach_get_position() {return mach.position;} + + +void mach_set_position(const float position[]) { + copy_vector(mach.position, position); +} + + /*** Get position of axis in absolute coordinates * * NOTE: Machine position is always returned in mm mode. No units conversion * is performed. */ -float mach_get_absolute_position(uint8_t axis) {return mach.position[axis];} +float mach_get_axis_position(uint8_t axis) {return mach.position[axis];} /* Critical helpers @@ -455,48 +474,41 @@ float mach_calc_move_time(const float axis_length[], * Axes that need processing are signaled in flag[] */ -// ESTEE: _calc_ABC is a fix to workaround a gcc compiler bug wherein it runs -// out of spill registers we moved this block into its own function so that we -// get a fresh stack push -// ALDEN: This shows up in avr-gcc 4.7.0 and avr-libc 1.8.0 -static float _calc_ABC(uint8_t axis, float target[], float flag[]) { - switch (axes[axis].axis_mode) { - case AXIS_STANDARD: - case AXIS_INHIBITED: - return target[axis]; // no mm conversion - it's in degrees - - default: - return TO_MILLIMETERS(target[axis]) * 360 / (2 * M_PI * axes[axis].radius); - } -} - - -void mach_set_model_target(float target[], float flag[]) { +void mach_calc_model_target(float target[], const float values[], + const float flags[]) { // process XYZABC for lower modes for (int axis = AXIS_X; axis <= AXIS_Z; axis++) { - if (fp_FALSE(flag[axis]) || axes[axis].axis_mode == AXIS_DISABLED) + if (fp_FALSE(flags[axis]) || axes[axis].axis_mode == AXIS_DISABLED) continue; // skip axis if not flagged for update or its disabled if (axes[axis].axis_mode == AXIS_STANDARD || axes[axis].axis_mode == AXIS_INHIBITED) { if (mach.gm.distance_mode == ABSOLUTE_MODE) - mach.gm.target[axis] = - mach_get_active_coord_offset(axis) + TO_MILLIMETERS(target[axis]); - else mach.gm.target[axis] += TO_MILLIMETERS(target[axis]); + target[axis] = + mach_get_active_coord_offset(axis) + TO_MILLIMETERS(values[axis]); + else target[axis] += TO_MILLIMETERS(values[axis]); } } - // NOTE: The ABC loop below relies on the XYZ loop having been run first + // Note: The ABC loop below relies on the XYZ loop having been run first for (int axis = AXIS_A; axis <= AXIS_C; axis++) { - if (fp_FALSE(flag[axis]) || axes[axis].axis_mode == AXIS_DISABLED) + if (fp_FALSE(flags[axis]) || axes[axis].axis_mode == AXIS_DISABLED) continue; // skip axis if not flagged for update or its disabled - float tmp = _calc_ABC(axis, target, flag); + float tmp; + switch (axes[axis].axis_mode) { + case AXIS_STANDARD: + case AXIS_INHIBITED: + tmp = values[axis]; // no mm conversion - it's in degrees + + default: + tmp = TO_MILLIMETERS(values[axis]) * 360 / (2 * M_PI * axes[axis].radius); + } if (mach.gm.distance_mode == ABSOLUTE_MODE) // sacidu93's fix to Issue #22 - mach.gm.target[axis] = tmp + mach_get_active_coord_offset(axis); - else mach.gm.target[axis] += tmp; + target[axis] = tmp + mach_get_active_coord_offset(axis); + else target[axis] += tmp; } } @@ -504,7 +516,7 @@ void mach_set_model_target(float target[], float flag[]) { /*** Return error code if soft limit is exceeded * * Must be called with target properly set in GM struct. Best done - * after mach_set_model_target(). + * after mach_calc_model_target(). * * Tests for soft limit for any homed axis if min and max are * different values. You can set min and max to 0,0 to disable soft @@ -627,7 +639,6 @@ void mach_set_axis_position(unsigned axis, float position) { if (AXES <= axis) return; mach.position[axis] = position; - mach.gm.target[axis] = position; mp_set_axis_position(axis, position); mp_runtime_set_axis_position(axis, position); mp_runtime_set_steps_from_position(); @@ -690,7 +701,6 @@ void mach_set_absolute_origin(float origin[], float flags[]) { if (fp_TRUE(flags[axis])) { value[axis] = TO_MILLIMETERS(origin[axis]); mach.position[axis] = value[axis]; // set model position - mach.gm.target[axis] = value[axis]; // reset model target mp_set_axis_position(axis, value[axis]); // set mm position } @@ -739,18 +749,20 @@ void mach_resume_origin_offsets() { // Free Space Motion (4.3.4) /// G0 linear rapid -stat_t mach_rapid(float target[], float flags[]) { +stat_t mach_rapid(float values[], float flags[]) { mach.gm.motion_mode = MOTION_MODE_RAPID; - mach_set_model_target(target, flags); + + float target[AXES]; + mach_calc_model_target(target, values, flags); // test soft limits - stat_t status = mach_test_soft_limits(mach.gm.target); + stat_t status = mach_test_soft_limits(target); if (status != STAT_OK) return ALARM(status); // prep and plan the move mach_update_work_offsets(); // update resolved offsets - status = mp_aline(mach.gm.target, mach_get_line()); // send move to planner - copy_vector(mach.position, mach.gm.target); // update model position + status = mp_aline(target, mach_get_line()); // send move to planner + copy_vector(mach.position, target); // update model position return status; } @@ -826,23 +838,25 @@ stat_t mach_dwell(float seconds) { /// G1 -stat_t mach_feed(float target[], float flags[]) { +stat_t mach_feed(float values[], float flags[]) { // trap zero feed rate condition if (fp_ZERO(mach.gm.feed_rate) || (mach.gm.feed_mode == INVERSE_TIME_MODE && !parser.gf.feed_rate)) return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; mach.gm.motion_mode = MOTION_MODE_FEED; - mach_set_model_target(target, flags); + + float target[AXES]; + mach_calc_model_target(target, values, flags); // test soft limits - stat_t status = mach_test_soft_limits(mach.gm.target); + stat_t status = mach_test_soft_limits(target); if (status != STAT_OK) return ALARM(status); // prep and plan the move - mach_update_work_offsets(); // update resolved offsets - status = mp_aline(mach.gm.target, mach_get_line()); // send move to planner - copy_vector(mach.position, mach.gm.target); // update model position + mach_update_work_offsets(); // update resolved offsets + status = mp_aline(target, mach_get_line()); // send move to planner + copy_vector(mach.position, target); // update model position return status; } diff --git a/src/machine.h b/src/machine.h index 10c3bc0..2c66840 100644 --- a/src/machine.h +++ b/src/machine.h @@ -36,31 +36,10 @@ #include -#define TO_MILLIMETERS(a) (mach.gm.units == INCHES ? (a) * MM_PER_INCH : a) - - -// Note 1: Our G0 omits G4, G30, G53, G92.1, G92.2, G92.3 as these have no axis -// components to error check - -typedef struct { // struct to manage mach globals and cycles - float offset[COORDS + 1][AXES]; // coordinate systems & offsets G53-G59 - float origin_offset[AXES]; // G92 offsets - bool origin_offset_enable; // G92 offsets enabled / disabled - - float position[AXES]; // model position - float g28_position[AXES]; // stored machine position for G28 - float g30_position[AXES]; // stored machine position for G30 - - gcode_state_t gm; // core gcode model state -} machine_t; - - -extern machine_t mach; // machine controller singleton - +#define TO_MILLIMETERS(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a) // Model state getters and setters uint32_t mach_get_line(); -uint8_t mach_get_tool(); float mach_get_feed_rate(); feed_mode_t mach_get_feed_mode(); float mach_get_feed_override(); @@ -90,11 +69,14 @@ void mach_set_model_line(uint32_t line); // Coordinate systems and offsets float mach_get_active_coord_offset(uint8_t axis); void mach_update_work_offsets(); -float mach_get_absolute_position(uint8_t axis); +const float *mach_get_position(); +void mach_set_position(const float position[]); +float mach_get_axis_position(uint8_t axis); // Critical helpers float mach_calc_move_time(const float axis_length[], const float axis_square[]); -void mach_set_model_target(float target[], float flag[]); +void mach_calc_model_target(float target[], const float values[], + const float flags[]); stat_t mach_test_soft_limits(float target[]); // machining functions defined by NIST [organized by NIST Gcode doc] diff --git a/src/plan/arc.c b/src/plan/arc.c index 6fff741..1874b13 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -171,8 +171,10 @@ static float _estimate_arc_time(float length, float linear_travel, */ static stat_t _compute_arc_offsets_from_radius(float offset[]) { // Calculate the change in position along each selected axis - float x = mach.gm.target[arc.plane_axis_0] - mach.position[arc.plane_axis_0]; - float y = mach.gm.target[arc.plane_axis_1] - mach.position[arc.plane_axis_1]; + float x = + arc.target[arc.plane_axis_0] - mach_get_axis_position(arc.plane_axis_0); + float y = + arc.target[arc.plane_axis_1] - mach_get_axis_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 @@ -340,7 +342,7 @@ static stat_t _compute_arc(bool radius_f, const float position[], * Generates an arc by queuing line segments to the move buffer. The arc is * approximated by generating a large number of tiny, linear segments. */ -stat_t mach_arc_feed(float target[], float target_f[], // arc endpoints +stat_t mach_arc_feed(float values[], float values_f[], // arc endpoints float offsets[], float offsets_f[], // arc offsets float radius, bool radius_f, // radius float P, bool P_f, // parameter @@ -397,7 +399,7 @@ stat_t mach_arc_feed(float target[], float target_f[], // arc endpoints // Test if endpoints are specified in the selected plane bool full_circle = false; - if (!target_f[arc.plane_axis_0] && !target_f[arc.plane_axis_1]) { + if (!values_f[arc.plane_axis_0] && !values_f[arc.plane_axis_1]) { if (radius_f) // in radius mode arcs missing both endpoints is an error return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; else full_circle = true; // in center format arc specifies full circle @@ -425,40 +427,39 @@ stat_t mach_arc_feed(float target[], float target_f[], // arc endpoints } else if (full_circle) rotations = 1; // default to 1 for full circles - // set values in Gcode model state & copy it (line was already captured) - mach_set_model_target(target, target_f); + // Set model target + mach_calc_model_target(arc.target, values, values_f); + const float *position = mach_get_position(); // in radius mode it's an error for start == end - if (radius_f && fp_EQ(mach.position[AXIS_X], mach.gm.target[AXIS_X]) && - fp_EQ(mach.position[AXIS_Y], mach.gm.target[AXIS_Y]) && - fp_EQ(mach.position[AXIS_Z], mach.gm.target[AXIS_Z])) + if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) && + fp_EQ(position[AXIS_Y], arc.target[AXIS_Y]) && + fp_EQ(position[AXIS_Z], arc.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 mach_set_motion_mode(motion_mode); mach_update_work_offsets(); // Update resolved offsets arc.line = mach_get_line(); // copy line number - copy_vector(arc.target, mach.gm.target); // copy move target arc.radius = TO_MILLIMETERS(radius); // set arc radius or zero float offset[3]; for (int i = 0; i < 3; i++) offset[i] = TO_MILLIMETERS(offsets[i]); if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) { - if (offsets_f[0]) offset[0] -= mach.position[0]; - if (offsets_f[1]) offset[1] -= mach.position[1]; - if (offsets_f[2]) offset[2] -= mach.position[2]; + if (offsets_f[0]) offset[0] -= position[0]; + if (offsets_f[1]) offset[1] -= position[1]; + if (offsets_f[2]) offset[2] -= position[2]; } // compute arc runtime values - RITORNO(_compute_arc(radius_f, mach.position, offset, rotations, - full_circle)); + RITORNO(_compute_arc(radius_f, position, offset, rotations, full_circle)); // Note, arc soft limits are not tested here arc.running = true; // Enable arc run in callback mach_arc_callback(); // Queue initial arc moves - copy_vector(mach.position, mach.gm.target); // update model position + mach_set_position(arc.target); // update model position return STAT_OK; } diff --git a/src/probing.c b/src/probing.c index 18796f3..ab7dcc3 100644 --- a/src/probing.c +++ b/src/probing.c @@ -118,7 +118,7 @@ static void _probing_finish() { mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); // store the probe results - pb.results[axis] = mach_get_absolute_position(axis); + pb.results[axis] = mach_get_axis_position(axis); } _probe_restore_settings(); @@ -159,7 +159,7 @@ static void _probing_init() { pb.saved_jerk[axis] = axes_get_jerk(axis); // save the max jerk value // use homing jerk for probe axes_set_jerk(axis, axes[axis].jerk_homing); - pb.start_position[axis] = mach_get_absolute_position(axis); + pb.start_position[axis] = mach_get_axis_position(axis); } // error if the probe target is too close to the current position