From 56ca93fb9f4d17524eb9d17cae1f619714bd7e09 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Thu, 8 Sep 2016 03:45:07 -0700 Subject: [PATCH] Report GCode state #18, Eliminated move_state_t struct, Reorg of spindle code --- src/command.c | 2 +- src/estop.c | 2 +- src/gcode_parser.c | 41 ++++--- src/homing.c | 12 +- src/machine.c | 258 +++++++++++++++++++++++++++---------------- src/machine.h | 50 ++++----- src/plan/arc.c | 51 +++++---- src/plan/buffer.c | 5 +- src/plan/buffer.h | 16 +-- src/plan/calibrate.c | 2 +- src/plan/command.c | 6 +- src/plan/dwell.c | 8 +- src/plan/exec.c | 7 +- src/plan/jog.c | 2 +- src/plan/line.c | 22 ++-- src/plan/line.h | 2 +- src/plan/planner.c | 2 +- src/plan/runtime.c | 28 +++++ src/probing.c | 10 +- src/probing.h | 2 +- src/spindle.c | 60 +++------- src/spindle.h | 7 +- src/stepper.c | 8 ++ src/varcb.c | 56 ++++++++-- src/vars.def | 20 +++- 25 files changed, 392 insertions(+), 287 deletions(-) diff --git a/src/command.c b/src/command.c index 16397d2..4a43ec3 100644 --- a/src/command.c +++ b/src/command.c @@ -227,7 +227,7 @@ void command_callback() { default: if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;} else if (mp_is_flushing()) break; // Flush GCode command - else if (!mp_get_planner_buffer_room() || + else if (mp_get_planner_buffer_room() < 2 || mp_is_resuming() || mach_arc_active() || mach_is_homing() || diff --git a/src/estop.c b/src/estop.c index 1d23c9e..6b2aea7 100644 --- a/src/estop.c +++ b/src/estop.c @@ -93,7 +93,7 @@ void estop_trigger(estop_reason_t reason) { // Hard stop the motors and the spindle st_shutdown(); - mach_spindle_estop(); + spindle_estop(); // Set machine state mp_state_estop(); diff --git a/src/gcode_parser.c b/src/gcode_parser.c index ed239a5..f07e73d 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -203,8 +203,7 @@ static stat_t _validate_gcode_block() { * 1. comment (includes message) [handled during block normalization] * 2. set feed rate mode (G93, G94 - inverse time or per minute) * 3. set feed rate (F) - * 3a. set feed override rate (M50.1) - * 3a. set traverse override rate (M50.2) + * 3a. set feed override rate (M50) * 4. set spindle speed (S) * 4a. set spindle override rate (M51.1) * 5. select tool (T) @@ -236,24 +235,22 @@ static stat_t _execute_gcode_block() { mach_set_model_line(mach.gn.line); EXEC_FUNC(mach_set_feed_rate_mode, feed_rate_mode); EXEC_FUNC(mach_set_feed_rate, feed_rate); - EXEC_FUNC(mach_feed_rate_override_factor, feed_rate_override_factor); - EXEC_FUNC(mach_traverse_override_factor, traverse_override_factor); + EXEC_FUNC(mach_feed_override_factor, feed_override_factor); EXEC_FUNC(mach_set_spindle_speed, spindle_speed); EXEC_FUNC(mach_spindle_override_factor, spindle_override_factor); EXEC_FUNC(mach_select_tool, tool_select); EXEC_FUNC(mach_change_tool, tool_change); - EXEC_FUNC(mach_spindle_control, spindle_mode); + EXEC_FUNC(mach_set_spindle_mode, spindle_mode); EXEC_FUNC(mach_mist_coolant_control, mist_coolant); EXEC_FUNC(mach_flood_coolant_control, flood_coolant); - EXEC_FUNC(mach_feed_rate_override_enable, feed_rate_override_enable); - EXEC_FUNC(mach_traverse_override_enable, traverse_override_enable); + EXEC_FUNC(mach_feed_override_enable, feed_override_enable); EXEC_FUNC(mach_spindle_override_enable, spindle_override_enable); EXEC_FUNC(mach_override_enables, override_enables); if (mach.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell RITORNO(mach_dwell(mach.gn.parameter)); - EXEC_FUNC(mach_set_plane, select_plane); + EXEC_FUNC(mach_set_plane, plane); EXEC_FUNC(mach_set_units_mode, units_mode); //--> cutter radius compensation goes here //--> cutter length compensation goes here @@ -285,7 +282,7 @@ static stat_t _execute_gcode_block() { mach_homing_cycle_start_no_set(); break; case NEXT_ACTION_STRAIGHT_PROBE: // G38.2 - status = mach_straight_probe(mach.gn.target, mach.gf.target); + status = mach_probe(mach.gn.target, mach.gf.target); break; case NEXT_ACTION_SET_COORD_DATA: mach_set_coord_offsets(mach.gn.parameter, mach.gn.target, mach.gf.target); @@ -306,17 +303,17 @@ static stat_t _execute_gcode_block() { case NEXT_ACTION_DEFAULT: // apply override setting to gm struct - mach_set_absolute_override(mach.gn.absolute_override); + mach_set_absolute_mode(mach.gn.absolute_mode); switch (mach.gn.motion_mode) { case MOTION_MODE_CANCEL_MOTION_MODE: mach.gm.motion_mode = mach.gn.motion_mode; break; - case MOTION_MODE_STRAIGHT_TRAVERSE: - status = mach_straight_traverse(mach.gn.target, mach.gf.target); + case MOTION_MODE_RAPID: + status = mach_rapid(mach.gn.target, mach.gf.target); break; - case MOTION_MODE_STRAIGHT_FEED: - status = mach_straight_feed(mach.gn.target, mach.gf.target); + case MOTION_MODE_FEED: + status = mach_feed(mach.gn.target, mach.gf.target); break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: // gf.radius sets radius mode if radius was collected in gn @@ -329,7 +326,7 @@ static stat_t _execute_gcode_block() { } } // un-set absolute override once the move is planned - mach_set_absolute_override(false); + mach_set_absolute_mode(false); // do the program stops and ends : M0, M1, M2, M30, M60 if (mach.gf.program_flow) @@ -374,17 +371,17 @@ static stat_t _parse_gcode_block(char *buf) { case 'G': switch ((uint8_t)value) { case 0: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE); + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID); case 1: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED); + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_FEED); case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC); case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC); case 4: SET_NON_MODAL(next_action, NEXT_ACTION_DWELL); case 10: SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA); - case 17: SET_MODAL(MODAL_GROUP_G2, select_plane, PLANE_XY); - case 18: SET_MODAL(MODAL_GROUP_G2, select_plane, PLANE_XZ); - case 19: SET_MODAL(MODAL_GROUP_G2, select_plane, PLANE_YZ); + case 17: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XY); + case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ); + case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ); case 20: SET_MODAL(MODAL_GROUP_G6, units_mode, INCHES); case 21: SET_MODAL(MODAL_GROUP_G6, units_mode, MILLIMETERS); case 28: @@ -419,7 +416,7 @@ static stat_t _parse_gcode_block(char *buf) { case 40: break; // ignore cancel cutter radius compensation case 49: break; // ignore cancel tool length offset comp. - case 53: SET_NON_MODAL(absolute_override, true); + case 53: SET_NON_MODAL(absolute_mode, true); case 54: SET_MODAL(MODAL_GROUP_G12, coord_system, G54); case 55: SET_MODAL(MODAL_GROUP_G12, coord_system, G55); case 56: SET_MODAL(MODAL_GROUP_G12, coord_system, G56); @@ -493,7 +490,7 @@ static stat_t _parse_gcode_block(char *buf) { case 9: SET_MODAL(MODAL_GROUP_M8, flood_coolant, false); // Also mist case 48: SET_MODAL(MODAL_GROUP_M9, override_enables, true); case 49: SET_MODAL(MODAL_GROUP_M9, override_enables, false); - case 50: SET_MODAL(MODAL_GROUP_M9, feed_rate_override_enable, true); + case 50: SET_MODAL(MODAL_GROUP_M9, feed_override_enable, true); case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); default: status = STAT_MCODE_COMMAND_UNSUPPORTED; } diff --git a/src/homing.c b/src/homing.c index 218d1fc..ee805b9 100644 --- a/src/homing.c +++ b/src/homing.c @@ -210,7 +210,7 @@ static void _homing_axis_move(int8_t axis, float target, float velocity) { mach.gm.feed_rate = velocity; mp_flush_planner(); // don't use mp_request_flush() here - stat_t status = mach_straight_feed(vect, flags); + stat_t status = mach_feed(vect, flags); if (status) _homing_error_exit(status); } @@ -379,11 +379,11 @@ void mach_set_homed(int axis, bool homed) { /// G28.2 homing cycle using limit switches void mach_homing_cycle_start() { // save relevant non-axis parameters from Gcode model - hm.saved_units_mode = mach_get_units_mode(&mach.gm); - hm.saved_coord_system = mach_get_coord_system(&mach.gm); - hm.saved_distance_mode = mach_get_distance_mode(&mach.gm); - hm.saved_feed_rate_mode = mach_get_feed_rate_mode(&mach.gm); - hm.saved_feed_rate = mach_get_feed_rate(&mach.gm); + hm.saved_units_mode = mach_get_units_mode(); + hm.saved_coord_system = mach_get_coord_system(); + hm.saved_distance_mode = mach_get_distance_mode(); + hm.saved_feed_rate_mode = mach_get_feed_rate_mode(); + hm.saved_feed_rate = mach_get_feed_rate(); // set working values mach_set_units_mode(MILLIMETERS); diff --git a/src/machine.c b/src/machine.c index 939bc9d..df1c04c 100644 --- a/src/machine.c +++ b/src/machine.c @@ -197,7 +197,7 @@ uint32_t mach_get_line() {return mach.gm.line;} motion_mode_t mach_get_motion_mode() {return mach.gm.motion_mode;} coord_system_t mach_get_coord_system() {return mach.gm.coord_system;} units_mode_t mach_get_units_mode() {return mach.gm.units_mode;} -plane_t mach_get_select_plane() {return mach.gm.select_plane;} +plane_t mach_get_plane() {return mach.gm.plane;} path_mode_t mach_get_path_control() {return mach.gm.path_control;} distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;} feed_rate_mode_t mach_get_feed_rate_mode() {return mach.gm.feed_rate_mode;} @@ -213,7 +213,65 @@ PGM_P mp_get_units_mode_pgmstr(units_mode_t mode) { case DEGREES: return PSTR("DEG"); } - return PSTR("invalid"); + return PSTR("INVALID"); +} + + +PGM_P mp_get_feed_rate_mode_pgmstr(feed_rate_mode_t mode) { + switch (mode) { + case INVERSE_TIME_MODE: return PSTR("INVERSE TIME"); + case UNITS_PER_MINUTE_MODE: return PSTR("PER MIN"); + case UNITS_PER_REVOLUTION_MODE: return PSTR("PER REV"); + } + + return PSTR("INVALID"); +} + + +PGM_P mp_get_plane_pgmstr(plane_t plane) { + switch (plane) { + case PLANE_XY: return PSTR("XY"); + case PLANE_XZ: return PSTR("XZ"); + case PLANE_YZ: return PSTR("YZ"); + } + + return PSTR("INVALID"); +} + + +PGM_P mp_get_coord_system_pgmstr(coord_system_t cs) { + switch (cs) { + case ABSOLUTE_COORDS: return PSTR("ABS"); + case G54: return PSTR("G54"); + case G55: return PSTR("G55"); + case G56: return PSTR("G56"); + case G57: return PSTR("G57"); + case G58: return PSTR("G58"); + case G59: return PSTR("G59"); + } + + return PSTR("INVALID"); +} + + +PGM_P mp_get_path_mode_pgmstr(path_mode_t mode) { + switch (mode) { + case PATH_EXACT_PATH: return PSTR("EXACT PATH"); + case PATH_EXACT_STOP: return PSTR("EXACT STOP"); + case PATH_CONTINUOUS: return PSTR("CONTINUOUS"); + } + + return PSTR("INVALID"); +} + + +PGM_P mp_get_distance_mode_pgmstr(distance_mode_t mode) { + switch (mode) { + case ABSOLUTE_MODE: return PSTR("ABSOLUTE"); + case INCREMENTAL_MODE: return PSTR("INCREMENTAL"); + } + + return PSTR("INVALID"); } @@ -222,23 +280,41 @@ void mach_set_motion_mode(motion_mode_t motion_mode) { } -void mach_set_spindle_mode(spindle_mode_t spindle_mode) { - mach.gm.spindle_mode = spindle_mode; +/// Spindle speed callback from planner queue +static void _exec_spindle_speed(float *value, float *flag) { + float speed = value[0]; + mach.gm.spindle_speed = speed; + spindle_set(mach.gm.spindle_mode, speed); } -void mach_set_spindle_speed_parameter(float speed) { - mach.gm.spindle_speed = speed; +/// Queue the S parameter to the planner buffer +void mach_set_spindle_speed(float speed) { + float value[AXES] = {speed}; + mp_queue_command(_exec_spindle_speed, value, value); +} + + +/// execute the spindle command (called from planner) +static void _exec_spindle_mode(float *value, float *flag) { + spindle_mode_t mode = value[0]; + mach.gm.spindle_mode = mode; + spindle_set(mode, mach.gm.spindle_speed); +} + + +/// Queue the spindle command to the planner buffer +void mach_set_spindle_mode(spindle_mode_t mode) { + float value[AXES] = {mode}; + mp_queue_command(_exec_spindle_mode, value, value); } void mach_set_tool_number(uint8_t tool) {mach.gm.tool = tool;} -void mach_set_absolute_override(bool absolute_override) { - mach.gm.absolute_override = absolute_override; - // must reset offsets if you change absolute override - mach_set_work_offsets(); +void mach_set_absolute_mode(bool absolute_mode) { + mach.gm.absolute_mode = absolute_mode; } @@ -306,13 +382,11 @@ void mach_set_axis_jerk(uint8_t axis, float jerk) { * Takes G5x, G92 and absolute override into account to return the * active offset for this move * - * This function is typically used to evaluate and set offsets, as - * opposed to mach_get_work_offset() which merely returns what's in the - * work_offset[] array. + * This function is typically used to evaluate and set offsets. */ float mach_get_active_coord_offset(uint8_t axis) { // no offset in absolute override mode - if (mach.gm.absolute_override) return 0; + if (mach.gm.absolute_mode) return 0; float offset = mach.offset[mach.gm.coord_system][axis]; if (mach.origin_offset_enable) @@ -322,16 +396,37 @@ float mach_get_active_coord_offset(uint8_t axis) { } -/// Return a coord offset -float mach_get_work_offset(uint8_t axis) { - return mach.ms.work_offset[axis]; +static stat_t _exec_update_work_offsets(mp_buffer_t *bf) { + mp_runtime_set_work_offsets(bf->target); + return STAT_OK; } // Capture coord offsets from the model into absolute values -void mach_set_work_offsets() { - for (int axis = 0; axis < AXES; axis++) - mach.ms.work_offset[axis] = mach_get_active_coord_offset(axis); +void mach_update_work_offsets() { + static float work_offset[AXES] = {0}; + bool same = true; + + for (int axis = 0; axis < AXES; axis++) { + float offset = mach_get_active_coord_offset(axis); + + if (offset != work_offset[axis]) { + work_offset[axis] = offset; + same = false; + } + } + + if (!same) { + mp_buffer_t *bf = mp_get_write_buffer(); + + // never supposed to fail + if (!bf) {CM_ALARM(STAT_BUFFER_FULL_FATAL); return;} + + bf->bf_func = _exec_update_work_offsets; + copy_vector(bf->target, work_offset); + + mp_commit_write_buffer(mach.gm.line); + } } @@ -366,7 +461,7 @@ float mach_get_work_position(uint8_t axis) { * These functions are not part of the NIST defined functions */ -/* Perform final operations for a traverse or feed +/* Perform final operations for a rapid or feed * * These routines set the point position in the gcode model. * @@ -378,12 +473,12 @@ float mach_get_work_position(uint8_t axis) { * close to the starting point. */ void mach_finalize_move() { - copy_vector(mach.position, mach.ms.target); // update model position + copy_vector(mach.position, mach.gm.target); // update model position // if in inverse time mode reset feed rate so next block requires an // explicit feed rate setting if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE && - mach.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED) + mach.gm.motion_mode == MOTION_MODE_FEED) mach.gm.feed_rate = 0; } @@ -399,7 +494,7 @@ void mach_finalize_move() { * * "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 + * achievable. Optimal times for rapids are always the minimum * time. * * The gcode state must have targets set prior by having @@ -454,7 +549,8 @@ void mach_finalize_move() { * from the start to the end of the motion is T plus any time * required for acceleration or deceleration. */ -void mach_calc_move_time(const float axis_length[], const float axis_square[]) { +float mach_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 @@ -462,14 +558,14 @@ void mach_calc_move_time(const float axis_length[], const float axis_square[]) { float tmp_time = 0; // used in computation // compute times for feed motion - if (mach.gm.motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) { + if (mach.gm.motion_mode != MOTION_MODE_RAPID) { if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) { // feed rate was un-inverted to minutes by mach_set_feed_rate() inv_time = mach.gm.feed_rate; mach.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; } else { - // compute length of linear move in millimeters. Feed rate is provided as + // 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]) / mach.gm.feed_rate; @@ -483,16 +579,16 @@ void mach_calc_move_time(const float axis_length[], const float axis_square[]) { } for (int axis = 0; axis < AXES; axis++) { - if (mach.gm.motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE) + if (mach.gm.motion_mode == MOTION_MODE_RAPID) tmp_time = fabs(axis_length[axis]) / mach.a[axis].velocity_max; - else // MOTION_MODE_STRAIGHT_FEED + else // MOTION_MODE_FEED tmp_time = fabs(axis_length[axis]) / mach.a[axis].feedrate_max; max_time = max(max_time, tmp_time); } - mach.ms.move_time = max4(inv_time, max_time, xyz_time, abc_time); + return max4(inv_time, max_time, xyz_time, abc_time); } @@ -541,9 +637,9 @@ void mach_set_model_target(float target[], float flag[]) { if (mach.a[axis].axis_mode == AXIS_STANDARD || mach.a[axis].axis_mode == AXIS_INHIBITED) { if (mach.gm.distance_mode == ABSOLUTE_MODE) - mach.ms.target[axis] = + mach.gm.target[axis] = mach_get_active_coord_offset(axis) + TO_MILLIMETERS(target[axis]); - else mach.ms.target[axis] += TO_MILLIMETERS(target[axis]); + else mach.gm.target[axis] += TO_MILLIMETERS(target[axis]); } } @@ -555,8 +651,8 @@ void mach_set_model_target(float target[], float flag[]) { if (mach.gm.distance_mode == ABSOLUTE_MODE) // sacidu93's fix to Issue #22 - mach.ms.target[axis] = tmp + mach_get_active_coord_offset(axis); - else mach.ms.target[axis] += tmp; + mach.gm.target[axis] = tmp + mach_get_active_coord_offset(axis); + else mach.gm.target[axis] += tmp; } } @@ -615,7 +711,7 @@ void machine_init() { mach_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // always the default // Sub-system inits - mach_spindle_init(); + spindle_init(); coolant_init(); } @@ -634,7 +730,7 @@ stat_t mach_alarm(const char *location, stat_t code) { // These functions assume input validation occurred upstream. /// G17, G18, G19 select axis plane -void mach_set_plane(plane_t plane) {mach.gm.select_plane = plane;} +void mach_set_plane(plane_t plane) {mach.gm.plane = plane;} /// G20, G21 @@ -650,14 +746,10 @@ void mach_set_distance_mode(distance_mode_t mode) { /* G10 L2 Pn, delayed persistence * * This function applies the offset to the GM model. - * - * It also does not reset the work_offsets which may be - * accomplished by calling mach_set_work_offsets() immediately - * afterwards. */ void mach_set_coord_offsets(coord_system_t coord_system, float offset[], float flag[]) { - if (coord_system < G54 || MAX_COORDS <= coord_system) return; + if (coord_system < G54 || G59 < coord_system) return; for (int axis = 0; axis < AXES; axis++) if (fp_TRUE(flag[axis])) @@ -668,7 +760,6 @@ void mach_set_coord_offsets(coord_system_t coord_system, float offset[], /// G54-G59 void mach_set_coord_system(coord_system_t coord_system) { mach.gm.coord_system = coord_system; - mach_set_work_offsets(); // set work offsets in the Gcode model } @@ -693,7 +784,7 @@ void mach_set_axis_position(unsigned axis, float position) { if (AXES <= axis) return; mach.position[axis] = position; - mach.ms.target[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(); @@ -751,8 +842,8 @@ void mach_set_absolute_origin(float origin[], float flag[]) { if (fp_TRUE(flag[axis])) { value[axis] = TO_MILLIMETERS(origin[axis]); mach.position[axis] = value[axis]; // set model position - mach.ms.target[axis] = value[axis]; // reset model target - mp_set_axis_position(axis, value[axis]); // set mm position + mach.gm.target[axis] = value[axis]; // reset model target + mp_set_axis_position(axis, value[axis]); // set mm position } mp_queue_command(_exec_absolute_origin, value, flag); @@ -771,8 +862,6 @@ void mach_set_origin_offsets(float offset[], float flag[]) { if (fp_TRUE(flag[axis])) mach.origin_offset[axis] = mach.position[axis] - mach.offset[mach.gm.coord_system][axis] - TO_MILLIMETERS(offset[axis]); - - mach_set_work_offsets(); // set work offsets in the Gcode model } @@ -781,40 +870,35 @@ void mach_reset_origin_offsets() { mach.origin_offset_enable = false; for (int axis = 0; axis < AXES; axis++) mach.origin_offset[axis] = 0; - - mach_set_work_offsets(); // set work offsets in the Gcode model } /// G92.2 void mach_suspend_origin_offsets() { mach.origin_offset_enable = false; - mach_set_work_offsets(); // set work offsets in the Gcode model } /// G92.3 void mach_resume_origin_offsets() { mach.origin_offset_enable = true; - mach_set_work_offsets(); // set work offsets in the Gcode model } // Free Space Motion (4.3.4) /// G0 linear rapid -stat_t mach_straight_traverse(float target[], float flags[]) { - mach.gm.motion_mode = MOTION_MODE_STRAIGHT_TRAVERSE; +stat_t mach_rapid(float target[], float flags[]) { + mach.gm.motion_mode = MOTION_MODE_RAPID; mach_set_model_target(target, flags); // test soft limits - stat_t status = mach_test_soft_limits(mach.ms.target); + stat_t status = mach_test_soft_limits(mach.gm.target); if (status != STAT_OK) return CM_ALARM(status); // prep and plan the move - mach_set_work_offsets(); // capture fully resolved offsets to state - mach.ms.line = mach.gm.line; // copy line number - status = mp_aline(&mach.ms); // send the move to the planner + mach_update_work_offsets(); // update fully resolved offsets to state + status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner mach_finalize_move(); return status; @@ -827,17 +911,17 @@ void mach_set_g28_position() {copy_vector(mach.g28_position, mach.position);} /// G28 stat_t mach_goto_g28_position(float target[], float flags[]) { - mach_set_absolute_override(true); + mach_set_absolute_mode(true); // move through intermediate point, or skip - mach_straight_traverse(target, flags); + mach_rapid(target, flags); // make sure you have an available buffer mp_wait_for_buffer(); // execute actual stored move float f[] = {1, 1, 1, 1, 1, 1}; - return mach_straight_traverse(mach.g28_position, f); + return mach_rapid(mach.g28_position, f); } @@ -847,17 +931,17 @@ void mach_set_g30_position() {copy_vector(mach.g30_position, mach.position);} /// G30 stat_t mach_goto_g30_position(float target[], float flags[]) { - mach_set_absolute_override(true); + mach_set_absolute_mode(true); // move through intermediate point, or skip - mach_straight_traverse(target, flags); + mach_rapid(target, flags); // make sure you have an available buffer mp_wait_for_buffer(); // execute actual stored move float f[] = {1, 1, 1, 1, 1, 1}; - return mach_straight_traverse(mach.g30_position, f); + return mach_rapid(mach.g30_position, f); } @@ -895,22 +979,21 @@ stat_t mach_dwell(float seconds) { /// G1 -stat_t mach_straight_feed(float target[], float flags[]) { +stat_t mach_feed(float target[], float flags[]) { // trap zero feed rate condition if (mach.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(mach.gm.feed_rate)) return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - mach.gm.motion_mode = MOTION_MODE_STRAIGHT_FEED; + mach.gm.motion_mode = MOTION_MODE_FEED; mach_set_model_target(target, flags); // test soft limits - stat_t status = mach_test_soft_limits(mach.ms.target); + stat_t status = mach_test_soft_limits(mach.gm.target); if (status != STAT_OK) return CM_ALARM(status); // prep and plan the move - mach_set_work_offsets(); // capture fully resolved offsets to state - mach.ms.line = mach.gm.line; // copy line number - status = mp_aline(&mach.ms); // send the move to the planner + mach_update_work_offsets(); // update fully resolved offsets to state + status = mp_aline(mach.gm.target, mach.gm.line); // send the move to planner mach_finalize_move(); return status; @@ -964,43 +1047,27 @@ void mach_flood_coolant_control(bool flood_coolant) { /// M48, M49 void mach_override_enables(bool flag) { - mach.gm.feed_rate_override_enable = flag; - mach.gm.traverse_override_enable = flag; + mach.gm.feed_override_enable = flag; mach.gm.spindle_override_enable = flag; } /// M50 -void mach_feed_rate_override_enable(bool flag) { +void mach_feed_override_enable(bool flag) { if (fp_TRUE(mach.gf.parameter) && fp_ZERO(mach.gn.parameter)) - mach.gm.feed_rate_override_enable = false; - else mach.gm.feed_rate_override_enable = true; + mach.gm.feed_override_enable = false; + else mach.gm.feed_override_enable = true; } -/// M50.1 -void mach_feed_rate_override_factor(bool flag) { - mach.gm.feed_rate_override_enable = flag; - mach.gm.feed_rate_override_factor = mach.gn.parameter; -} - - -/// M50.2 -void mach_traverse_override_enable(bool flag) { - if (fp_TRUE(mach.gf.parameter) && fp_ZERO(mach.gn.parameter)) - mach.gm.traverse_override_enable = false; - else mach.gm.traverse_override_enable = true; +/// M50 +void mach_feed_override_factor(bool flag) { + mach.gm.feed_override_enable = flag; + mach.gm.feed_override_factor = mach.gn.parameter; } /// M51 -void mach_traverse_override_factor(bool flag) { - mach.gm.traverse_override_enable = flag; - mach.gm.traverse_override_factor = mach.gn.parameter; -} - - -/// M51.1 void mach_spindle_override_enable(bool flag) { if (fp_TRUE(mach.gf.parameter) && fp_ZERO(mach.gn.parameter)) mach.gm.spindle_override_enable = false; @@ -1008,7 +1075,7 @@ void mach_spindle_override_enable(bool flag) { } -/// M50.1 +/// M51 void mach_spindle_override_factor(bool flag) { mach.gm.spindle_override_enable = flag; mach.gm.spindle_override_factor = mach.gn.parameter; @@ -1101,7 +1168,8 @@ void mach_program_end() { mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); mach_set_plane(GCODE_DEFAULT_PLANE); mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); - mach_spindle_control(SPINDLE_OFF); // M5 + mach.gm.spindle_mode = SPINDLE_OFF; + spindle_set(SPINDLE_OFF, 0); mach_flood_coolant_control(false); // M9 mach_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); // G94 mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); diff --git a/src/machine.h b/src/machine.h index 4814b8d..6d2ba47 100644 --- a/src/machine.h +++ b/src/machine.h @@ -67,8 +67,8 @@ typedef enum { typedef enum { // G Modal Group 1 - MOTION_MODE_STRAIGHT_TRAVERSE, // G0 - straight traverse - MOTION_MODE_STRAIGHT_FEED, // G1 - straight feed + MOTION_MODE_RAPID, // G0 - rapid + MOTION_MODE_FEED, // G1 - straight feed MOTION_MODE_CW_ARC, // G2 - clockwise arc feed MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed MOTION_MODE_CANCEL_MOTION_MODE, // G80 @@ -127,7 +127,6 @@ typedef enum { typedef enum { ABSOLUTE_COORDS, // machine coordinate system G54, G55, G56, G57, G58, G59, - MAX_COORDS, } coord_system_t; /// G Modal Group 13 @@ -223,14 +222,6 @@ typedef enum { */ -typedef struct { - int32_t line; // gcode block line number - 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 -} move_state_t; - - /// Gcode model state typedef struct { uint32_t line; // Gcode block line number @@ -240,10 +231,8 @@ typedef struct { float feed_rate; // F - in mm/min or inverse time mode feed_rate_mode_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 feed_override_factor; // 1.0000 x F feed rate + bool feed_override_enable; // M48, M49 float spindle_speed; // in RPM spindle_mode_t spindle_mode; @@ -251,10 +240,10 @@ typedef struct { bool spindle_override_enable; // true = override enabled motion_mode_t motion_mode; // Group 1 modal motion - plane_t select_plane; // G17, G18, G19 + plane_t plane; // G17, G18, G19 units_mode_t units_mode; // G20, G21 coord_system_t coord_system; // G54-G59 - select coordinate system 1-9 - bool absolute_override; // G53 true = move in machine coordinates + bool absolute_mode; // G53 true = move in machine coordinates path_mode_t path_control; // G61 distance_mode_t distance_mode; // G91 distance_mode_t arc_distance_mode; // G91.1 @@ -309,7 +298,6 @@ typedef struct { // struct to manage mach globals and cycles float g30_position[AXES]; // stored machine position for G30 axis_config_t a[AXES]; // settings for axes - move_state_t ms; gcode_state_t gm; // core gcode model state gcode_state_t gn; // gcode input values gcode_state_t gf; // gcode input flags @@ -324,7 +312,7 @@ uint32_t mach_get_line(); motion_mode_t mach_get_motion_mode(); coord_system_t mach_get_coord_system(); units_mode_t mach_get_units_mode(); -plane_t mach_get_select_plane(); +plane_t mach_get_plane(); path_mode_t mach_get_path_control(); distance_mode_t mach_get_distance_mode(); feed_rate_mode_t mach_get_feed_rate_mode(); @@ -334,12 +322,17 @@ inline float mach_get_spindle_speed() {return mach.gm.spindle_speed;} float mach_get_feed_rate(); PGM_P mp_get_units_mode_pgmstr(units_mode_t mode); +PGM_P mp_get_feed_rate_mode_pgmstr(feed_rate_mode_t mode); +PGM_P mp_get_plane_pgmstr(plane_t plane); +PGM_P mp_get_coord_system_pgmstr(coord_system_t cs); +PGM_P mp_get_path_mode_pgmstr(path_mode_t mode); +PGM_P mp_get_distance_mode_pgmstr(distance_mode_t mode); void mach_set_motion_mode(motion_mode_t motion_mode); void mach_set_spindle_mode(spindle_mode_t spindle_mode); -void mach_set_spindle_speed_parameter(float speed); +void mach_set_spindle_speed(float speed); void mach_set_tool_number(uint8_t tool); -void mach_set_absolute_override(bool absolute_override); +void mach_set_absolute_mode(bool absolute_mode); void mach_set_model_line(uint32_t line); float mach_get_axis_jerk(uint8_t axis); @@ -347,13 +340,12 @@ void mach_set_axis_jerk(uint8_t axis, float jerk); // Coordinate systems and offsets float mach_get_active_coord_offset(uint8_t axis); -float mach_get_work_offset(uint8_t axis); -void mach_set_work_offsets(); +void mach_update_work_offsets(); float mach_get_absolute_position(uint8_t axis); float mach_get_work_position(uint8_t axis); // Critical helpers -void mach_calc_move_time(const float axis_length[], const float axis_square[]); +float mach_calc_move_time(const float axis_length[], const float axis_square[]); void mach_finalize_move(); stat_t mach_deferred_write_callback(); void mach_set_model_target(float target[], float flag[]); @@ -390,7 +382,7 @@ void mach_suspend_origin_offsets(); void mach_resume_origin_offsets(); // Free Space Motion (4.3.4) -stat_t mach_straight_traverse(float target[], float flags[]); +stat_t mach_rapid(float target[], float flags[]); void mach_set_g28_position(); stat_t mach_goto_g28_position(float target[], float flags[]); void mach_set_g30_position(); @@ -402,7 +394,7 @@ void mach_set_feed_rate_mode(feed_rate_mode_t mode); void mach_set_path_control(path_mode_t mode); // Machining Functions (4.3.6) -stat_t mach_straight_feed(float target[], float flags[]); +stat_t mach_feed(float target[], float flags[]); stat_t mach_arc_feed(float target[], float flags[], float i, float j, float k, float radius, uint8_t motion_mode); stat_t mach_dwell(float seconds); @@ -418,10 +410,8 @@ void mach_mist_coolant_control(bool mist_coolant); void mach_flood_coolant_control(bool flood_coolant); void mach_override_enables(bool flag); -void mach_feed_rate_override_enable(bool flag); -void mach_feed_rate_override_factor(bool flag); -void mach_traverse_override_enable(bool flag); -void mach_traverse_override_factor(bool flag); +void mach_feed_override_enable(bool flag); +void mach_feed_override_factor(bool flag); void mach_spindle_override_enable(bool flag); void mach_spindle_override_factor(bool flag); diff --git a/src/plan/arc.c b/src/plan/arc.c index 027a1f3..74ef1dd 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -74,7 +74,8 @@ typedef struct { 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) - move_state_t ms; + int32_t line; // gcode block line number + float target[AXES]; // XYZABC where the move should go } arc_t; arc_t arc = {0}; @@ -192,8 +193,8 @@ static void _estimate_arc_time() { */ static stat_t _compute_arc_offsets_from_radius() { // Calculate the change in position along each selected axis - float x = mach.ms.target[arc.plane_axis_0] - mach.position[arc.plane_axis_0]; - float y = mach.ms.target[arc.plane_axis_1] - mach.position[arc.plane_axis_1]; + 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]; // *** From Forrest Green - Other Machine Co, 3/27/14 // If the distance between endpoints is greater than the arc diameter, disc @@ -263,9 +264,9 @@ static stat_t _compute_arc() { // (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." // Compute end radius from the center of circle (offsets) to target endpoint - float end_0 = arc.ms.target[arc.plane_axis_0] - + float end_0 = arc.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0]; - float end_1 = arc.ms.target[arc.plane_axis_1] - + float end_1 = arc.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); @@ -282,7 +283,7 @@ static stat_t _compute_arc() { // g18_correction is used to invert G18 XZ plane arcs for proper CW // orientation - float g18_correction = mach.gm.select_plane == PLANE_XZ ? -1 : 1; + float g18_correction = mach.gm.plane == PLANE_XZ ? -1 : 1; if (arc.full_circle) { // angular travel always starts as zero for full circles @@ -321,7 +322,7 @@ static stat_t _compute_arc() { // should take to perform the move arc.length is the total mm of travel of // the helix (or just a planar arc) arc.linear_travel = - arc.ms.target[arc.linear_axis] - arc.position[arc.linear_axis]; + arc.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); @@ -342,15 +343,13 @@ static stat_t _compute_arc() { arc_segments_for_minimum_time)); arc.arc_segments = max(arc.arc_segments, 1); // at least 1 arc_segment - // gcode state struct gets arc_segment_time, not arc time - arc.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.ms.target[arc.linear_axis] = arc.position[arc.linear_axis]; + arc.target[arc.linear_axis] = arc.position[arc.linear_axis]; return STAT_OK; } @@ -390,7 +389,8 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints // specification Plane axis 0 and 1 are the arc plane, the linear axis is // normal to the arc plane. // G17 - the vast majority of arcs are in the G17 (XY) plane - if (mach.gm.select_plane == PLANE_XY) { + switch (mach.gm.plane) { + case PLANE_XY: arc.plane_axis_0 = AXIS_X; arc.plane_axis_1 = AXIS_Y; arc.linear_axis = AXIS_Z; @@ -404,8 +404,9 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints // center format arc tests, it's OK to be missing either or both i and j, // but error if k is present return STAT_ARC_SPECIFICATION_ERROR; + break; - } else if (mach.gm.select_plane == PLANE_XZ) { // G18 + case PLANE_XZ: // G18 arc.plane_axis_0 = AXIS_X; arc.plane_axis_1 = AXIS_Z; arc.linear_axis = AXIS_Y; @@ -414,8 +415,9 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints if (!(target_x || target_z)) return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; } else if (offset_j) return STAT_ARC_SPECIFICATION_ERROR; + break; - } else if (mach.gm.select_plane == PLANE_YZ) { // G19 + case PLANE_YZ: // G19 arc.plane_axis_0 = AXIS_Y; arc.plane_axis_1 = AXIS_Z; arc.linear_axis = AXIS_X; @@ -424,22 +426,23 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints if (!target_y && !target_z) return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; } else if (offset_i) return STAT_ARC_SPECIFICATION_ERROR; + break; } // set values in Gcode model state & copy it (line was already captured) mach_set_model_target(target, flags); // in radius mode it's an error for start == end - if (radius_f && fp_EQ(mach.position[AXIS_X], mach.ms.target[AXIS_X]) && - fp_EQ(mach.position[AXIS_Y], mach.ms.target[AXIS_Y]) && - fp_EQ(mach.position[AXIS_Z], mach.ms.target[AXIS_Z])) + 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])) return STAT_ARC_ENDPOINT_IS_STARTING_POINT; // now get down to the rest of the work setting up the arc for execution mach.gm.motion_mode = motion_mode; - mach_set_work_offsets(); // Update resolved offsets - mach.ms.line = mach.gm.line; // copy line number - memcpy(&arc.ms, &mach.ms, sizeof(move_state_t)); // context to arc singleton + mach_update_work_offsets(); // Update resolved offsets + arc.line = mach.gm.line; // copy line number + copy_vector(arc.target, mach.gm.target); // copy move target copy_vector(arc.position, mach.position); // arc pos from gcode model @@ -479,14 +482,14 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints void mach_arc_callback() { while (arc.run_state != MOVE_OFF && mp_get_planner_buffer_room()) { arc.theta += arc.arc_segment_theta; - arc.ms.target[arc.plane_axis_0] = + arc.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius; - arc.ms.target[arc.plane_axis_1] = + arc.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; - arc.ms.target[arc.linear_axis] += arc.arc_segment_linear_travel; + arc.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 + mp_aline(arc.target, arc.line); // run the line + copy_vector(arc.position, arc.target); // update arc current pos if (!--arc.arc_segment_count) arc.run_state = MOVE_OFF; } diff --git a/src/plan/buffer.c b/src/plan/buffer.c index e090ed4..1192fb5 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -139,11 +139,10 @@ mp_buffer_t *mp_get_write_buffer() { * buffer once it has been queued. Action may start on the buffer immediately, * invalidating its contents */ -void mp_commit_write_buffer(uint32_t line, move_type_t type) { +void mp_commit_write_buffer(uint32_t line) { mp_state_running(); - mb.q->ms.line = line; - mb.q->move_type = type; + mb.q->line = line; mb.q->run_state = MOVE_NEW; mb.q->buffer_state = MP_BUFFER_QUEUED; mb.q = mb.q->nx; // advance the queued buffer pointer diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 4288ad3..56c63bb 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -33,14 +33,6 @@ #include -typedef enum { - MOVE_TYPE_NULL, // null move - does a no-op - MOVE_TYPE_ALINE, // acceleration planned line - MOVE_TYPE_DWELL, // delay with no movement - MOVE_TYPE_COMMAND, // general command -} move_type_t; - - typedef enum { MOVE_OFF, // move inactive (MUST BE ZERO) MOVE_NEW, // initial value @@ -73,10 +65,12 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes mach_func_t mach_func; // callback to machine buffer_state_t buffer_state; // used to manage queuing/dequeuing - move_type_t move_type; // used to dispatch to run routine run_state_t run_state; // run state machine sequence bool replannable; // true if move can be re-planned + int32_t line; // gcode block line number + + float target[AXES]; // XYZABC where the move should go float unit[AXES]; // unit vector for axis scaling & planning float length; // total length of line or helix in mm @@ -99,7 +93,7 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes float recip_jerk; // 1/Jm used for planning (computed & cached) float cbrt_jerk; // cube root of Jm (computed & cached) - move_state_t ms; + float dwell; } mp_buffer_t; @@ -108,7 +102,7 @@ void mp_wait_for_buffer(); void mp_init_buffers(); bool mp_queue_empty(); mp_buffer_t *mp_get_write_buffer(); -void mp_commit_write_buffer(uint32_t line, move_type_t type); +void mp_commit_write_buffer(uint32_t line); mp_buffer_t *mp_get_run_buffer(); void mp_free_run_buffer(); mp_buffer_t *mp_get_last_buffer(); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index 5bf4344..23cb6fe 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -176,7 +176,7 @@ uint8_t command_calibrate(int argc, char *argv[]) { cal.motor = 1; bf->bf_func = _exec_calibrate; // register callback - mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND); + mp_commit_write_buffer(-1); return 0; } diff --git a/src/plan/command.c b/src/plan/command.c index 3d0f2ca..80bb4c6 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -53,7 +53,7 @@ /// Callback to execute command static stat_t _exec_command(mp_buffer_t *bf) { - st_prep_command(bf->mach_func, bf->ms.target, bf->unit); + st_prep_command(bf->mach_func, bf->target, bf->unit); return STAT_OK; // Done } @@ -72,10 +72,10 @@ void mp_queue_command(mach_func_t mach_func, float values[], float flags[]) { // Store values and flags in planner buffer for (int axis = 0; axis < AXES; axis++) { - bf->ms.target[axis] = values[axis]; + bf->target[axis] = values[axis]; bf->unit[axis] = flags[axis]; // flag vector in unit } // Must be final operation before exit - mp_commit_write_buffer(mach_get_line(), MOVE_TYPE_COMMAND); + mp_commit_write_buffer(mach_get_line()); } diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 5728131..1141bd4 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -39,7 +39,7 @@ /// Dwell execution static stat_t _exec_dwell(mp_buffer_t *bf) { - st_prep_dwell(bf->ms.move_time); // in seconds + st_prep_dwell(bf->dwell); // in seconds return STAT_OK; // Done } @@ -51,11 +51,11 @@ stat_t mp_dwell(float seconds, int32_t line) { // never supposed to fail if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); - bf->bf_func = _exec_dwell; // register callback to dwell start - bf->ms.move_time = seconds; // in seconds, not minutes + bf->bf_func = _exec_dwell; // register callback to dwell start + bf->dwell = seconds; // in seconds, not minutes // must be final operation before exit - mp_commit_write_buffer(line, MOVE_TYPE_DWELL); + mp_commit_write_buffer(line); return STAT_OK; } diff --git a/src/plan/exec.c b/src/plan/exec.c index 1f6d112..cae6332 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -402,7 +402,7 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { // Initialize move copy_vector(ex.unit, bf->unit); - copy_vector(ex.final_target, bf->ms.target); + copy_vector(ex.final_target, bf->target); ex.head_length = bf->head_length; ex.body_length = bf->body_length; @@ -415,9 +415,6 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { ex.section_new = true; ex.hold_planned = false; - // Update runtime - mp_runtime_set_work_offsets(bf->ms.work_offset); - // Generate waypoints for position correction at section ends. This helps // negate floating point errors in the forward differencing code. for (int axis = 0; axis < AXES; axis++) { @@ -532,7 +529,7 @@ stat_t mp_exec_move() { // Update runtime mp_runtime_set_busy(true); - mp_runtime_set_line(bf->ms.line); + mp_runtime_set_line(bf->line); } stat_t status = bf->bf_func(bf); // Move callback diff --git a/src/plan/jog.c b/src/plan/jog.c index 2d25c94..d8c5aee 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -132,7 +132,7 @@ uint8_t command_jog(int argc, char *argv[]) { // Start mp_set_cycle(CYCLE_JOGGING); bf->bf_func = _exec_jog; // register callback - mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND); + mp_commit_write_buffer(-1); } return STAT_OK; diff --git a/src/plan/line.c b/src/plan/line.c index 4376a04..0794de8 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -178,14 +178,14 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { * accumulate and get executed once the accumulated error exceeds * the minimums. */ -stat_t mp_aline(move_state_t *ms) { +stat_t mp_aline(const float target[], int32_t line) { // Compute some reusable terms float axis_length[AXES]; float axis_square[AXES]; float length_square = 0; for (int axis = 0; axis < AXES; axis++) { - axis_length[axis] = ms->target[axis] - mm.position[axis]; + axis_length[axis] = target[axis] - mm.position[axis]; axis_square[axis] = square(axis_length[axis]); length_square += axis_square[axis]; } @@ -200,8 +200,8 @@ stat_t mp_aline(move_state_t *ms) { // minimum move time get a more accurate time estimate based on // starting velocity and acceleration. The time of the move is // determined by its initial velocity (Vi) and how much acceleration - // will occur. For this we need to look at the exit velocity of - // the previous block. There are 3 possible cases: + // will occur. For this we need to look at the exit velocity of + // the previous block. There are 3 possible cases: // // (1) There is no previous block. // Vi = 0 @@ -213,9 +213,9 @@ stat_t mp_aline(move_state_t *ms) { // Vi <= previous block's entry_velocity + delta_velocity // Set move time in state - mach_calc_move_time(axis_length, axis_square); + float move_time = mach_calc_move_time(axis_length, axis_square); - if (ms->move_time < MIN_BLOCK_TIME) { + if (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 @@ -228,7 +228,7 @@ stat_t mp_aline(move_state_t *ms) { } // Compute execution time for this move - float move_time = 2 * length / (2 * entry_velocity + delta_velocity); + move_time = 2 * length / (2 * entry_velocity + delta_velocity); if (move_time < MIN_BLOCK_TIME) return STAT_MINIMUM_TIME_MOVE; } @@ -241,7 +241,7 @@ stat_t mp_aline(move_state_t *ms) { bf->length = length; // Copy model state into planner buffer - memcpy(&bf->ms, ms, sizeof(move_state_t)); + copy_vector(bf->target, target); // Compute the unit vector and find the right jerk to use (combined // operations) To determine the jerk value to use for the block we @@ -361,7 +361,7 @@ stat_t mp_aline(move_state_t *ms) { // finish up the current block variables float junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); - bf->cruise_vmax = bf->length / bf->ms.move_time; // target velocity requested + bf->cruise_vmax = bf->length / move_time; // target velocity requested bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); bf->exit_vmax = @@ -371,10 +371,10 @@ stat_t mp_aline(move_state_t *ms) { // NOTE: these next lines must remain in exact order. Position must update // before committing the buffer. mp_plan_block_list(bf); // plan block list - copy_vector(mm.position, bf->ms.target); // set the planner position + copy_vector(mm.position, target); // set the planner position // commit current block (must follow the position update) - mp_commit_write_buffer(ms->line, MOVE_TYPE_ALINE); + mp_commit_write_buffer(line); return STAT_OK; } diff --git a/src/plan/line.h b/src/plan/line.h index 75ce7d5..d2ccbf0 100644 --- a/src/plan/line.h +++ b/src/plan/line.h @@ -30,4 +30,4 @@ #include "machine.h" void mp_set_axis_position(int axis, float position); -stat_t mp_aline(move_state_t *ms); +stat_t mp_aline(const float target[], int32_t line); diff --git a/src/plan/planner.c b/src/plan/planner.c index 49eb6da..e3dfada 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -473,7 +473,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { * Variables that are ignored but here's what you would expect them to be: * * bf->run_state - NEW for all blocks but the earliest - * bf->ms.target[] - block target position + * bf->target[] - block target position * bf->unit[] - block unit vector * bf->time - gets set later * bf->jerk - source of the other jerk variables. diff --git a/src/plan/runtime.c b/src/plan/runtime.c index 04dc93a..c12274e 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -51,6 +51,33 @@ typedef struct { } mp_steps_t; +typedef struct { + int32_t line; // Current move GCode line number + uint8_t tool; + + float feed; + feed_rate_mode_t feed_rate_mode; + float feed_override; + + float speed; + spindle_mode_t spindle_mode; + float spindle_override; + + motion_mode_t motion_mode; + plane_t plane; + units_mode_t units_mode; + coord_system_t coord_system; + bool absolute_mode; + path_mode_t path_control; + distance_mode_t distance_mode; + distance_mode_t arc_distance_mode; + + bool mist_coolant; + bool flood_coolant; + +} mach_state_t; + + typedef struct { bool busy; // True if a move is running int32_t line; // Current move GCode line number @@ -58,6 +85,7 @@ typedef struct { float work_offset[AXES]; // Current move work offset float velocity; // Current move velocity mp_steps_t steps; + mach_state_t mach; } mp_runtime_t; static mp_runtime_t rt; diff --git a/src/probing.c b/src/probing.c index 989ab43..ee76982 100644 --- a/src/probing.c +++ b/src/probing.c @@ -129,7 +129,7 @@ static void _probing_start() { bool closed = switch_is_active(SW_PROBE); if (!closed) { - stat_t status = mach_straight_feed(pb.target, pb.flags); + stat_t status = mach_feed(pb.target, pb.flags); if (status) return _probing_error_exit(status); } @@ -172,12 +172,12 @@ static void _probing_init() { _probing_error_exit(STAT_MOVE_DURING_PROBE); // probe in absolute machine coords - pb.saved_coord_system = mach_get_coord_system(&mach.gm); - pb.saved_distance_mode = mach_get_distance_mode(&mach.gm); + pb.saved_coord_system = mach_get_coord_system(); + pb.saved_distance_mode = mach_get_distance_mode(); mach_set_distance_mode(ABSOLUTE_MODE); mach_set_coord_system(ABSOLUTE_COORDS); - mach_spindle_control(SPINDLE_OFF); + mach_set_spindle_mode(SPINDLE_OFF); // start the move pb.func = _probing_start; @@ -190,7 +190,7 @@ bool mach_is_probing() { /// G38.2 homing cycle using limit switches -stat_t mach_straight_probe(float target[], float flags[]) { +stat_t mach_probe(float target[], float flags[]) { // trap zero feed rate condition if (mach.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(mach.gm.feed_rate)) return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; diff --git a/src/probing.h b/src/probing.h index 3c58203..870eed6 100644 --- a/src/probing.h +++ b/src/probing.h @@ -33,5 +33,5 @@ bool mach_is_probing(); -stat_t mach_straight_probe(float target[], float flags[]); +stat_t mach_probe(float target[], float flags[]); void mach_probe_callback(); diff --git a/src/spindle.c b/src/spindle.c index 9209b0b..5cc12f5 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -40,70 +40,38 @@ typedef enum { SPINDLE_TYPE_HUANYANG, } spindle_type_t; -static spindle_type_t spindle_type = SPINDLE_TYPE; +static spindle_type_t _type = SPINDLE_TYPE; -static void _spindle_set(spindle_mode_t mode, float speed) { - switch (spindle_type) { - case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break; - case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break; - } -} - - -/// execute the spindle command (called from planner) -static void _exec_spindle_control(float *value, float *flag) { - spindle_mode_t mode = value[0]; - mach_set_spindle_mode(mode); - _spindle_set(mode, mach.gm.spindle_speed); -} - - -/// Spindle speed callback from planner queue -static void _exec_spindle_speed(float *value, float *flag) { - float speed = value[0]; - mach_set_spindle_speed_parameter(speed); - _spindle_set(mach.gm.spindle_mode, speed); -} - - -void mach_spindle_init() { +void spindle_init() { pwm_spindle_init(); huanyang_init(); } -/// Queue the spindle command to the planner buffer -void mach_spindle_control(spindle_mode_t mode) { - float value[AXES] = {mode}; - mp_queue_command(_exec_spindle_control, value, value); +void spindle_set(spindle_mode_t mode, float speed) { + switch (_type) { + case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break; + } } -void mach_spindle_estop() { - switch (spindle_type) { +void spindle_estop() { + switch (_type) { case SPINDLE_TYPE_PWM: pwm_spindle_estop(); break; case SPINDLE_TYPE_HUANYANG: huanyang_estop(); break; } } -/// Queue the S parameter to the planner buffer -void mach_set_spindle_speed(float speed) { - float value[AXES] = {speed}; - mp_queue_command(_exec_spindle_speed, value, value); -} - - -uint8_t get_spindle_type(int index) { - return spindle_type; -} +uint8_t get_spindle_type(int index) {return _type;} void set_spindle_type(int index, uint8_t value) { - if (value != spindle_type) { - _spindle_set(SPINDLE_OFF, 0); - spindle_type = value; - _spindle_set(mach.gm.spindle_mode, mach.gm.spindle_speed); + if (value != _type) { + spindle_set(SPINDLE_OFF, 0); + _type = value; + spindle_set(mach.gm.spindle_mode, mach.gm.spindle_speed); } } diff --git a/src/spindle.h b/src/spindle.h index f4313c7..c31ec85 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -31,7 +31,6 @@ #include "machine.h" -void mach_spindle_init(); -void mach_set_spindle_speed(float speed); // S parameter -void mach_spindle_control(spindle_mode_t spindle_mode); // M3, M4, M5 -void mach_spindle_estop(); +void spindle_init(); +void spindle_set(spindle_mode_t spindle_mode, float speed); +void spindle_estop(); diff --git a/src/stepper.c b/src/stepper.c index 112f54f..eec89cd 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -44,6 +44,14 @@ #include +typedef enum { + MOVE_TYPE_NULL, // null move - does a no-op + MOVE_TYPE_ALINE, // acceleration planned line + MOVE_TYPE_DWELL, // delay with no movement + MOVE_TYPE_COMMAND, // general command +} move_type_t; + + typedef struct { // Runtime bool busy; diff --git a/src/varcb.c b/src/varcb.c index d48e2a6..333ce1e 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -29,20 +29,62 @@ #include "usart.h" #include "machine.h" #include "plan/runtime.h" -#include "plan/planner.h" #include "plan/state.h" -#include "plan/buffer.h" - +// Axis float get_position(int index) {return mp_runtime_get_axis_position(index);} -float get_velocity() {return mp_runtime_get_velocity();} + +// GCode +int32_t get_line() {return mp_runtime_get_line();} +PGM_P get_unit() {return mp_get_units_mode_pgmstr(mach_get_units_mode());} float get_speed() {return mach_get_spindle_speed();} float get_feed() {return mach_get_feed_rate();} uint8_t get_tool() {return mach_get_tool();} + + +PGM_P get_feed_rate_mode() { + return mp_get_feed_rate_mode_pgmstr(mach.gm.feed_rate_mode); +} + + +PGM_P get_plane() {return mp_get_plane_pgmstr(mach.gm.plane);} + + +PGM_P get_coord_system() { + return mp_get_coord_system_pgmstr(mach.gm.coord_system); +} + + +bool get_abs_override() {return mach.gm.absolute_mode;} +PGM_P get_path_control() {return mp_get_path_mode_pgmstr(mach.gm.path_control);} + + +PGM_P get_distance_mode() { + return mp_get_distance_mode_pgmstr(mach.gm.distance_mode); +} + + +PGM_P get_arc_dist_mode() { + return mp_get_distance_mode_pgmstr(mach.gm.arc_distance_mode); +} + + +float get_feed_override() { + return mach.gm.feed_override_enable ? mach.gm.feed_override_factor : 0; +} + + +float get_speed_override() { + return mach.gm.spindle_override_enable ? mach.gm.spindle_override_factor : 0; +} + + +bool get_mist_coolant() {return mach.gm.mist_coolant;} +bool get_flood_coolant() {return mach.gm.flood_coolant;} + +// System +float get_velocity() {return mp_runtime_get_velocity();} bool get_echo() {return usart_is_set(USART_ECHO);} void set_echo(bool value) {return usart_set(USART_ECHO, value);} -uint16_t get_queue() {return mp_get_planner_buffer_room();} -int32_t get_line() {return mp_runtime_get_line();} PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());} PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());} -PGM_P get_unit() {return mp_get_units_mode_pgmstr(mach_get_units_mode());} diff --git a/src/vars.def b/src/vars.def index 70d44f3..03a4727 100644 --- a/src/vars.def +++ b/src/vars.def @@ -91,17 +91,29 @@ VAR(huanyang_connected, "he", bool, 0, 0, 0, "Huanyang connected") // Switches VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 1, "Normally open or closed") -// System +// GCode +VAR(line, "ln", int32_t, 0, 0, 0, "Last GCode line executed") VAR(unit, "u", pstring, 0, 0, 0, "Current unit of measure") -VAR(velocity, "v", float, 0, 0, 0, "Current velocity") VAR(speed, "s", float, 0, 0, 0, "Current spindle speed") VAR(feed, "f", float, 0, 0, 0, "Current feed rate") VAR(tool, "t", uint8_t, 0, 0, 0, "Current tool") +VAR(feed_rate_mode, "fm", pstring, 0, 0, 0, "Current feed rate mode") +VAR(plane, "pa", pstring, 0, 0, 0, "Current plane") +VAR(coord_system, "cs", pstring, 0, 0, 0, "Current coordinate system") +VAR(abs_override, "ao", bool, 0, 0, 0, "Absolute override enabled") +VAR(path_control, "pc", pstring, 0, 0, 0, "Current path control mode") +VAR(distance_mode, "dm", pstring, 0, 0, 0, "Current distance mode") +VAR(arc_dist_mode, "ad", pstring, 0, 0, 0, "Current arc distance mode") +VAR(mist_coolant, "mc", bool, 0, 0, 0, "Mist coolant enabled") +VAR(flood_coolant, "fc", bool, 0, 0, 0, "Flood coolant enabled") +VAR(feed_override, "fo", float, 0, 0, 0, "Feed rate override") +VAR(speed_override, "so", float, 0, 0, 0, "Spindle speed override") + +// System +VAR(velocity, "v", float, 0, 0, 0, "Current velocity") VAR(hw_id, "id", string, 0, 0, 0, "Hardware ID") VAR(echo, "ec", bool, 0, 1, 0, "Enable or disable echo") VAR(estop, "es", bool, 0, 1, 0, "Emergency stop") VAR(estop_reason, "er", pstring, 0, 0, 0, "Emergency stop reason") -VAR(line, "ln", int32_t, 0, 0, 0, "Last GCode line executed") -VAR(queue, "q", uint16_t, 0, 0, 0, "Space in planner queue") VAR(state, "x", pstring, 0, 0, 0, "Machine state") VAR(cycle, "c", pstring, 0, 0, 0, "Machine cycle") -- 2.27.0