From e6ad15b548c65bf64c5fc733329e0ed9ebfc0e27 Mon Sep 17 00:00:00 2001 From: Sonny Jeon Date: Sat, 10 Nov 2012 12:49:33 -0700 Subject: [PATCH] Tweaks. Seek rate updates when set. CCW arc full circle fix. - Fixed a minor issue where the seek rates would not immediately be used and only would after a reset. Should update live now. - A full circle IJ offset CCW arc would not do anything. Fixed bug via a simple if-then statement. - Radius mode tweaks to check for negative value in sqrt() rather than isnan() it. Error report updated to indicate what actually happened. --- gcode.c | 18 ++++++++++-------- gcode.h | 2 +- motion_control.c | 7 +++++-- report.c | 4 ++-- report.h | 2 +- 5 files changed, 19 insertions(+), 14 deletions(-) diff --git a/gcode.c b/gcode.c index 691118f..029e022 100755 --- a/gcode.c +++ b/gcode.c @@ -52,7 +52,7 @@ void gc_init() { memset(&gc, 0, sizeof(gc)); gc.feed_rate = settings.default_feed_rate; // Should be zero at initialization. - gc.seek_rate = settings.default_seek_rate; +// gc.seek_rate = settings.default_seek_rate; select_plane(X_AXIS, Y_AXIS, Z_AXIS); gc.absolute_mode = true; @@ -323,14 +323,14 @@ uint8_t gc_execute_line(char *line) target[i] = gc.position[i]; } } - mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false); + mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false); } // Retreive G28/30 go-home position data (in machine coordinates) from EEPROM float coord_data[N_AXIS]; uint8_t home_select = SETTING_INDEX_G28; if (non_modal_action == NON_MODAL_GO_HOME_1) { home_select = SETTING_INDEX_G30; } if (!settings_read_coord_data(home_select,coord_data)) { return(STATUS_SETTING_READ_FAIL); } - mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], gc.seek_rate, false); + mc_line(coord_data[X_AXIS], coord_data[Y_AXIS], coord_data[Z_AXIS], settings.default_seek_rate, false); axis_words = 0; // Axis words used. Lock out from motion modes by clearing flags. break; case NON_MODAL_SET_HOME_0: case NON_MODAL_SET_HOME_1: @@ -400,7 +400,7 @@ uint8_t gc_execute_line(char *line) break; case MOTION_MODE_SEEK: if (!axis_words) { FAIL(STATUS_INVALID_STATEMENT);} - else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], gc.seek_rate, false); } + else { mc_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], settings.default_seek_rate, false); } break; case MOTION_MODE_LINEAR: // TODO: Inverse time requires F-word with each statement. Need to do a check. Also need @@ -474,10 +474,12 @@ uint8_t gc_execute_line(char *line) float y = target[gc.plane_axis_1]-gc.position[gc.plane_axis_1]; clear_vector(offset); - float h_x2_div_d = -sqrt(4 * r*r - x*x - y*y)/hypot(x,y); // == -(h * 2 / d) - // If r is smaller than d, the arc is now traversing the complex plane beyond the reach of any - // real CNC, and thus - for practical reasons - we will terminate promptly: - if(isnan(h_x2_div_d)) { FAIL(STATUS_FLOATING_POINT_ERROR); return(gc.status_code); } + // First, use h_x2_div_d to compute 4*h^2 to check if it is negative or r is smaller + // than d. If so, the sqrt of a negative number is complex and error out. + float h_x2_div_d = 4 * r*r - x*x - y*y; + if (h_x2_div_d < 0) { FAIL(STATUS_ARC_RADIUS_ERROR); return(gc.status_code); } + // Finish computing h_x2_div_d. + h_x2_div_d = -sqrt(h_x2_div_d)/hypot(x,y); // == -(h * 2 / d) // Invert the sign of h_x2_div_d if the circle is counter clockwise (see sketch below) if (gc.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d; } diff --git a/gcode.h b/gcode.h index 4618744..8315ed2 100755 --- a/gcode.h +++ b/gcode.h @@ -83,7 +83,7 @@ typedef struct { int8_t spindle_direction; // 1 = CW, -1 = CCW, 0 = Stop {M3, M4, M5} uint8_t coolant_mode; // 0 = Disable, 1 = Flood Enable {M8, M9} float feed_rate; // Millimeters/min - float seek_rate; // Millimeters/min - Can change depending on switches and such. +// float seek_rate; // Millimeters/min. Will be used in v0.9 when axis independence is installed float position[3]; // Where the interpreter considers the tool to be at this point in the code uint8_t tool; // uint16_t spindle_speed; // RPM/100 diff --git a/motion_control.c b/motion_control.c index 27c2cc5..c80e557 100755 --- a/motion_control.c +++ b/motion_control.c @@ -105,8 +105,11 @@ void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8 // CCW angle between position and target from circle center. Only one atan2() trig computation required. float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1); - if (angular_travel < 0) { angular_travel += 2*M_PI; } - if (isclockwise) { angular_travel -= 2*M_PI; } + if (isclockwise) { // Correct atan2 output per direction + if (angular_travel >= 0) { angular_travel -= 2*M_PI; } + } else { + if (angular_travel <= 0) { angular_travel += 2*M_PI; } + } float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel)); if (millimeters_of_travel == 0.0) { return; } diff --git a/report.c b/report.c index 16b906d..f5dd4f6 100644 --- a/report.c +++ b/report.c @@ -56,8 +56,8 @@ void report_status_message(uint8_t status_code) printPgmString(PSTR("Expected command letter")); break; case STATUS_UNSUPPORTED_STATEMENT: printPgmString(PSTR("Unsupported statement")); break; - case STATUS_FLOATING_POINT_ERROR: - printPgmString(PSTR("Float error")); break; + case STATUS_ARC_RADIUS_ERROR: + printPgmString(PSTR("Invalid radius")); break; case STATUS_MODAL_GROUP_VIOLATION: printPgmString(PSTR("Modal group violation")); break; case STATUS_INVALID_STATEMENT: diff --git a/report.h b/report.h index 4d8c471..b79e1af 100644 --- a/report.h +++ b/report.h @@ -26,7 +26,7 @@ #define STATUS_BAD_NUMBER_FORMAT 1 #define STATUS_EXPECTED_COMMAND_LETTER 2 #define STATUS_UNSUPPORTED_STATEMENT 3 -#define STATUS_FLOATING_POINT_ERROR 4 +#define STATUS_ARC_RADIUS_ERROR 4 #define STATUS_MODAL_GROUP_VIOLATION 5 #define STATUS_INVALID_STATEMENT 6 #define STATUS_HARD_LIMIT 7