diff -ruN src/emc/nml_intf/canon.hh src/emc/nml_intf/canon.hh --- src/emc/nml_intf/canon.hh 2009-01-29 11:55:58.000000000 -0600 +++ src/emc/nml_intf/canon.hh 2009-02-27 15:09:23.000000000 -0600 @@ -14,6 +14,7 @@ #define CANON_HH #include // FILE +#include /* This is the header file that all applications that use the @@ -56,6 +57,18 @@ #define OFF 0 #define ON 1 +typedef struct { /* type for NURBS control points */ + double X, + Y, + W; + } CONTROL_POINT; + +typedef struct { + double X, + Y; + } PLANE_POINT; + + typedef int CANON_PLANE; #define CANON_PLANE_XY 1 #define CANON_PLANE_YZ 2 @@ -423,6 +436,36 @@ double a, double b, double c, double u, double v, double w); +/* Additional functions needed to calculate nurbs points */ + +extern std::vector knot_vector_creator(unsigned int n, unsigned int k); +extern double Nmix(unsigned int i, unsigned int k, double u, + std::vector knot_vector); +extern double Rden(double u, unsigned int k, + std::vector nurbs_control_points, + std::vector knot_vector); +extern PLANE_POINT nurbs_point(double u, unsigned int k, + std::vector nurbs_control_points, + std::vector knot_vector); +extern double alpha_finder(double dx, double dy); + +/* Canon calls */ + +extern void NURBS_FEED(std::vector nurbs_control_points, unsigned int k); +/* Move at the feed rate along an approximation of a NURBS with a variable number + * of control points + */ + +extern void SPLINE_FEED(double x1, double y1, double x2, double y2, double x3, double y3); +/* Move at the feed rate along an approximation of the cubic + * bezier spline with control points x1y1 x2y2 and endpoint x3y3 + */ +extern void SPLINE_FEED(double x1, double y1, double x2, double y2); +/* Move at the feed rate along an approximation of the conic + * spline with control point x1y1 and endpoint x2y2 + */ + + /* Move at existing feed rate so that at any time during the move, all axes have covered the same proportion of their required motion. The meanings of the parameters is the same as for STRAIGHT_TRAVERSE.*/ diff -ruN src/emc/rs274ngc/Submakefile src/emc/rs274ngc/Submakefile --- src/emc/rs274ngc/Submakefile 2009-01-07 16:34:31.000000000 -0600 +++ src/emc/rs274ngc/Submakefile 2009-02-27 15:09:23.000000000 -0600 @@ -14,6 +14,7 @@ interp_read.cc \ interp_write.cc \ interp_o_word.cc \ + nurbs_additional_functions.cc \ rs274ngc_errors.cc \ rs274ngc_pre.cc) USERSRCS += $(LIBRS274SRCS) diff -ruN src/emc/rs274ngc/gcodemodule.cc src/emc/rs274ngc/gcodemodule.cc --- src/emc/rs274ngc/gcodemodule.cc 2009-01-29 11:56:00.000000000 -0600 +++ src/emc/rs274ngc/gcodemodule.cc 2009-02-28 00:41:34.000000000 -0600 @@ -178,6 +178,71 @@ Py_XDECREF(result); } +static double TO_PROG_LEN(double p) { + if(metric) return p*25.4; + return p; +} +static double FROM_PROG_LEN(double p) { + if(metric) return p/25.4; + return p; +} + +void NURBS_FEED(std::vector nurbs_control_points, unsigned int k) { + double u = 0.0; + unsigned int n = nurbs_control_points.size() - 1; + double umax = n - k + 2; + unsigned int div = nurbs_control_points.size()*15; + std::vector knot_vector = knot_vector_creator(n, k); + PLANE_POINT P1; + while (u+umax/div < umax) { + PLANE_POINT P1 = nurbs_point(u+umax/div,k,nurbs_control_points,knot_vector); + // EBo -- replace 12345 with *whatever* gives us the line_number + STRAIGHT_FEED(12345, P1.X,P1.Y, _pos_z, _pos_a, _pos_b, _pos_c, _pos_u, _pos_v, _pos_w); + u = u + umax/div; + } + P1.X = nurbs_control_points[n].X; + P1.Y = nurbs_control_points[n].Y; + // EBo -- replace 12345 with *whatever* gives us the line_number + STRAIGHT_FEED(12345, P1.X,P1.Y, _pos_z, _pos_a, _pos_b, _pos_c, _pos_u, _pos_v, _pos_w); + knot_vector.clear(); +} + +void SPLINE_FEED(double x1, double y1, double x2, double y2) { + double x0 = TO_PROG_LEN(_pos_x), + y0 = TO_PROG_LEN(_pos_y); + + fprintf(stderr, "SPLINE_FEED(conic): %8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", + x0,y0,x1,y1,x2,y2); + + for(int i=1; i<=100; i++) { + double t = i / 100.; + double t2 = t*t; + double t1 = 2*t*(1-t); + double t0 = (1-t)*(1-t); + double x = x0*t0 + x1*t1 + x2*t2; + double y = y0*t0 + y1*t1 + y2*t2; + // EBo -- replace 12345 with *whatever* gives us the line_number + STRAIGHT_FEED(12345, x,y, _pos_z, _pos_a, _pos_b, _pos_c, _pos_u, _pos_v, _pos_w); + } +} + +void SPLINE_FEED(double x1, double y1, double x2, double y2, double x3, double y3) { + double x0 = TO_PROG_LEN(_pos_x), + y0 = TO_PROG_LEN(_pos_y); + + for(int i=1; i<=100; i++) { double t = i / 100.; + double t3 = t*t*t; + double t2 = 3*t*t*(1-t); + double t1 = 3*t*(1-t)*(1-t); + double t0 = (1-t)*(1-t)*(1-t); + double x = x0*t0 + x1*t1 + x2*t2 + x3*t3; + double y = y0*t0 + y1*t1 + y2*t2 + y3*t3; + // EBo -- replace 12345 with *whatever* gives us the line_number + STRAIGHT_FEED(12345, x,y, _pos_z, _pos_a, _pos_b, _pos_c, _pos_u, _pos_v, _pos_w); + } +} + + void maybe_new_line(void) { maybe_new_line(-1); } diff -ruN src/emc/rs274ngc/interp_array.cc src/emc/rs274ngc/interp_array.cc --- src/emc/rs274ngc/interp_array.cc 2009-01-29 11:56:00.000000000 -0600 +++ src/emc/rs274ngc/interp_array.cc 2009-02-27 15:16:39.000000000 -0600 @@ -73,7 +73,8 @@ const int Interp::_gees[] = { /* 0 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 20 */ 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1,-1,-1,-1,-1,-1,-1,-1,-1,-1, -/* 40 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 40 */ //0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, +/* 40 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1, 0,-1,-1,-1,-1,-1,-1, /* 60 */ -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,15,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 80 */ 15,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, /* 100 */ 0,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, diff -ruN src/emc/rs274ngc/interp_check.cc src/emc/rs274ngc/interp_check.cc --- src/emc/rs274ngc/interp_check.cc 2009-01-29 11:56:00.000000000 -0600 +++ src/emc/rs274ngc/interp_check.cc 2009-02-27 15:43:17.000000000 -0600 @@ -78,12 +78,12 @@ setup_pointer settings) //!< pointer to machine settings { static char name[] = "check_g_codes"; - int mode0; + int mode0, mode1; int p_int; mode0 = block->g_modes[0]; - - if (mode0 == -1) { + mode1 = block->g_modes[1]; + if ((mode0 == -1) && (settings->motion_mode != G_5_2)) { } else if (mode0 == G_4) { CHK((block->p_number == -1.0), NCE_DWELL_TIME_MISSING_WITH_G4); } else if (mode0 == G_10) { @@ -93,6 +93,12 @@ CHKS(((block->l_number == 2 && ((p_int < 1) || (p_int > 9)))), "P value out of range with G10 L2"); CHKS(((block->l_number == 1 && ((p_int < 1) || (p_int > CANON_TOOL_MAX)))), "P value out of range with G10 L1"); } else if (mode0 == G_28) { + } else if (mode0 == G_28) { + } else if ((mode1 != G_5_2) && (mode0 != G_5_3)) { + CHKF(((mode1 != G_5_2) && (mode0 != G_5_3) && (mode1 != -1)), ( + "Can use only G5.2 or G5.3 after G5.2")); + } else if (mode1 == G_5_2){ + } else if (mode0 == G_5_3){ } else if (mode0 == G_30) { } else if (mode0 == G_28_1 || mode0 == G_30_1) { } else if (mode0 == G_53) { @@ -259,12 +265,14 @@ } if (block->i_flag == ON) { /* could still be useless if yz_plane arc */ - CHKS(((motion != G_2) && (motion != G_3) && (motion != G_76) && (motion != G_87) && (block->g_modes[8] != G_43_1)), + CHKS(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && + (motion != G_76) && (motion != G_87) && (block->g_modes[8] != G_43_1)), "I word with no G2, G3, G76, G87, or G43.1 to use it"); } if (block->j_flag == ON) { /* could still be useless if xz_plane arc */ - CHK(((motion != G_2) && (motion != G_3) && (motion != G_76) && (motion != G_87)), + CHK(((motion != G_2) && (motion != G_3) && (motion != G_5) && (motion != G_5_1) && + (motion != G_76) && (motion != G_87)), NCE_J_WORD_WITH_NO_G2_OR_G3_G76_OR_G87_TO_USE_IT); } @@ -289,14 +297,16 @@ if (block->l_number != -1) { CHKS((((motion < G_81) || (motion > G_89)) && (block->g_modes[0] != G_10) && - (motion != G_76) && +// (motion != G_76) && + (motion != G_76) && (motion != G_5_2) && (motion != G_73) && (block->g_modes[7] != G_41) && (block->g_modes[7] != G_41_1) && (block->g_modes[7] != G_42) && (block->g_modes[7] != G_42_1) && (block->m_modes[5] != 66)), - "L word with no G10, cutter compensation, or canned cycle"); +// "L word with no G10, cutter compensation, or canned cycle"); + "L word with no G10, cutter compensation, or canned cycle or NURBS code"); } if (block->p_number != -1.0) { @@ -316,12 +326,16 @@ (block->m_modes[9] != 53) && (block->user_m != 1) && (motion != G_82) && (motion != G_86) && - (motion != G_88) && (motion != G_89) && (motion != G_76)), +// (motion != G_88) && (motion != G_89) && (motion != G_76)), + (motion != G_88) && (motion != G_89) && (motion != G_76) && (motion != G_5) && + (motion != G_5_2)), + NCE_P_WORD_WITH_NO_G4_G10_G64_G76_G82_G86_G88_G89); } if (block->q_number != -1.0) { - CHKS((motion != G_83) && (motion != G_73) && (block->user_m != 1) && (motion != G_76) && +// CHKS((motion != G_83) && (motion != G_73) && (block->user_m != 1) && (motion != G_76) && + CHKS((motion != G_83) && (motion != G_5) && (block->user_m != 1) && (motion != G_76) && (block->m_modes[5] != 66) && (block->g_modes[0] != G_10) && (block->m_modes[6] != 61), "Q word with no g code that uses it"); } diff -ruN src/emc/rs274ngc/interp_convert.cc src/emc/rs274ngc/interp_convert.cc --- src/emc/rs274ngc/interp_convert.cc 2009-02-11 20:40:17.000000000 -0600 +++ src/emc/rs274ngc/interp_convert.cc 2009-02-27 15:43:58.000000000 -0600 @@ -126,6 +126,161 @@ /****************************************************************************/ +/*! convert_nurbs + * + * Returned value: int + * Returns a rs274ngc error code, or INTERP_OK if everything is OK. + * + * Side effects: Generates a nurbs move and updates the position of the tool + */ + +static unsigned int nurbs_order; +static std::vector nurbs_control_points; + +int Interp::convert_nurbs(int mode, + block_pointer block, //!< pointer to a block of RS274 instructions + setup_pointer settings) //!< pointer to machine settings +{ + static char name[] = "convert_nurbs"; + double end_z, AA_end, BB_end, CC_end, u_end, v_end, w_end; + CONTROL_POINT CP; + + if (mode == G_5_2) { + CHKF((((block->x_flag) && !(block->y_flag)) || (!(block->x_flag) && (block->y_flag))), ( + "You must specify both X and Y coordinates for Control Points")); + CHKF((!(block->x_flag) && !(block->y_flag) && (block->p_number > 0) && + (!nurbs_control_points.empty())), ( + "Can specify P without X and Y only for the first control point")); + + CHKF(((block->p_number <= 0) && (!nurbs_control_points.empty())), ( + "Must specify positive weight P for every Control Point")); + if (settings->feed_mode == UNITS_PER_MINUTE) { + CHKF((settings->feed_rate == 0.0), ( + "Cannot make a NURBS with 0 feedrate")); + } + if (nurbs_control_points.empty()) { + CP.X = settings->current_x; + CP.Y = settings->current_y; + if (!(block->x_flag) && !(block->y_flag) && (block->p_number > 0)) { + CP.W = block->p_number; + } else { + CP.W = 1; + } + nurbs_order = 3; + nurbs_control_points.push_back(CP); + } + if (block->l_number != -1 && block->l_number > 3) { + nurbs_order = block->l_number; + } + if ((block->x_flag) && (block->y_flag)) { + find_ends(block, settings, &CP.X, &CP.Y, &end_z, &AA_end, &BB_end, &CC_end, + &u_end, &v_end, &w_end); + CP.W = block->p_number; + nurbs_control_points.push_back(CP); + } + +//for (i=0;imotion_mode = mode; + } + + else if (mode == G_5_3){ + CHKF((settings->motion_mode != G_5_2), ( + "Cannot use G.3 without G.2 first")); + CHKF((nurbs_control_points.size()current_x = nurbs_control_points[nurbs_control_points.size()-1].X; + settings->current_y = nurbs_control_points[nurbs_control_points.size()-1].Y; + NURBS_FEED(nurbs_control_points, nurbs_order); + //printf("hello\n"); + nurbs_control_points.clear(); + //printf("%d\n", nurbs_control_points.size()); + settings->motion_mode = -1; + } + return INTERP_OK; +} + + /****************************************************************************/ + +/*! convert_spline + * + * Returned value: int + * Returns a rs274ngc error code, or INTERP_OK if everything is OK. + * + * Side effects: Generates a spline move and updates the position of the tool + */ +int Interp::convert_spline(int mode, + block_pointer block, //!< pointer to a block of RS274 instructions + setup_pointer settings) //!< pointer to machine settings +{ + static char name[] = "convert_spline"; + double x1, y1, x2, y2, x3, y3; + double end_z, AA_end, BB_end, CC_end, u_end, v_end, w_end; + + + CHKF((settings->cutter_comp_side != OFF), ( + "Cannot convert spline with cutter radius compensation")); + + if (settings->feed_mode == UNITS_PER_MINUTE) { + CHK((settings->feed_rate == 0.0), + NCE_CANNOT_MAKE_ARC_WITH_ZERO_FEED_RATE); + } else if (settings->feed_mode == INVERSE_TIME) { + CHK((block->f_number == -1.0), + NCE_F_WORD_MISSING_WITH_INVERSE_TIME_ARC_MOVE); + } + + CHKF((settings->plane != CANON_PLANE_XY), + ("Splines must be in the XY plane")); + //Error (for now): Splines must be in XY plane + + CHKF((block->z_flag == ON || block->a_flag == ON || block->b_flag == ON + || block->c_flag == ON), + ("Splines may not have motion in Z, A, B, or C")); + + if(mode == G_5_1) { + CHKF(block->i_flag != ON || block->j_flag != ON, + ("Must specify both I and J with G5.1")); + x1 = settings->current_x + block->i_number; + y1 = settings->current_y + block->j_number; + find_ends(block, settings, &x2, &y2, &end_z, &AA_end, &BB_end, &CC_end, + &u_end, &v_end, &w_end); + SPLINE_FEED(x1,y1,x2,y2); + settings->current_x = x2; + settings->current_y = y2; + } else { + if(block->i_flag != ON || block->j_flag != ON) { + CHKF(block->i_flag == ON || block->j_flag == ON, + ("Must specify both I and J, or neither")); + x1 = settings->current_x + settings->cycle_i; + y1 = settings->current_y + settings->cycle_j; + } else { + x1 = settings->current_x + block->i_number; + y1 = settings->current_y + block->j_number; + } + find_ends(block, settings, &x3, &y3, &end_z, &AA_end, &BB_end, &CC_end, + &u_end, &v_end, &w_end); + + x2 = x3 + block->p_number; + y2 = y3 + block->q_number; + + SPLINE_FEED(x1, y1, x2, y2, x3, y3); + + settings->cycle_i = -block->p_number; + settings->cycle_j = -block->q_number; + settings->current_x = x3; + settings->current_y = y3; + } + return INTERP_OK; +} + +/****************************************************************************/ + /*! convert_arc Returned Value: int @@ -2707,6 +2862,8 @@ } else if ((code == G_92) || (code == G_92_1) || (code == G_92_2) || (code == G_92_3)) { CHP(convert_axis_offsets(code, block, settings)); + } else if (code == G_5_3) { + CHP(convert_nurbs(code, block, settings)); } else if ((code == G_4) || (code == G_53)); /* handled elsewhere */ else ERM(NCE_BUG_CODE_NOT_G4_G10_G28_G30_G53_OR_G92_SERIES); @@ -2757,6 +2914,10 @@ settings->motion_mode = G_80; } else if (motion == G_73 || ((motion > G_80) && (motion < G_90))) { CHP(convert_cycle(motion, block, settings)); + } else if ((motion == G_5) || (motion == G_5_1)) { + CHP(convert_spline(motion, block, settings)); + } else if (motion == G_5_2) { + CHP(convert_nurbs(motion, block, settings)); } else { ERM(NCE_BUG_UNKNOWN_MOTION_CODE); } diff -ruN src/emc/rs274ngc/interp_internal.cc src/emc/rs274ngc/interp_internal.cc --- src/emc/rs274ngc/interp_internal.cc 2009-01-12 15:04:45.000000000 -0600 +++ src/emc/rs274ngc/interp_internal.cc 2009-02-27 15:26:10.000000000 -0600 @@ -167,7 +167,8 @@ } else { CHK(mode_zero_covets_axes, NCE_CANNOT_USE_TWO_G_CODES_THAT_BOTH_USE_AXIS_VALUES); - CHK(((!axis_flag) && (mode1 != G_0) && (mode1 != G_1) && mode1 != G_2 && mode1 != G_3), +// CHK(((!axis_flag) && (mode1 != G_0) && (mode1 != G_1) && mode1 != G_2 && mode1 != G_3), + CHK(((!axis_flag) && (mode1 != G_0) && (mode1 != G_1) && (mode1 != G_2) && (mode1 != G_3) && (mode1 != G_5_2)), NCE_ALL_AXES_MISSING_WITH_MOTION_CODE); } block->motion_to_be = mode1; diff -ruN src/emc/rs274ngc/interp_internal.hh src/emc/rs274ngc/interp_internal.hh --- src/emc/rs274ngc/interp_internal.hh 2009-02-03 16:08:45.000000000 -0600 +++ src/emc/rs274ngc/interp_internal.hh 2009-02-28 00:17:03.000000000 -0600 @@ -129,6 +129,10 @@ #define G_2 20 #define G_3 30 #define G_4 40 +#define G_5 50 +#define G_5_1 51 +#define G_5_2 52 +#define G_5_3 53 #define G_7 70 #define G_8 80 #define G_10 100 diff -ruN src/emc/rs274ngc/nurbs_additional_functions.cc src/emc/rs274ngc/nurbs_additional_functions.cc --- src/emc/rs274ngc/nurbs_additional_functions.cc 1969-12-31 18:00:00.000000000 -0600 +++ src/emc/rs274ngc/nurbs_additional_functions.cc 2009-02-27 15:31:47.000000000 -0600 @@ -0,0 +1,89 @@ +/* Those functions are needed to calculate NURBS points */ + + +#include +#include "canon.hh" + +std::vector knot_vector_creator(unsigned int n, unsigned int k) { + + unsigned int i; + std::vector knot_vector; + for (i=0; i<=n+k; i++) { + if (i < k) + knot_vector.push_back(0); + else if (i >= k && i <= n) + knot_vector.push_back(i - k + 1); + else + knot_vector.push_back(n - k + 2); + } + return knot_vector; + +} + +double Nmix(unsigned int i, unsigned int k, double u, + std::vector knot_vector) { + + if (k == 1){ + if ((u >= knot_vector[i]) && (u <= knot_vector[i+1])) { + return 1; + } else { + return 0;} + } else if (k > 1) { + if ((knot_vector[i+k-1]-knot_vector[i] == 0) && + (knot_vector[i+k]-knot_vector[i+1] != 0)) { + return ((knot_vector[i+k] - u)*Nmix(i+1,k-1,u,knot_vector))/ + (knot_vector[i+k]-knot_vector[i+1]); + } else if ((knot_vector[i+k]-knot_vector[i+1] == 0) && + (knot_vector[i+k-1]-knot_vector[i] != 0)) { + return ((u - knot_vector[i])*Nmix(i,k-1,u,knot_vector))/ + (knot_vector[i+k-1]-knot_vector[i]); + } else if ((knot_vector[i+k-1]-knot_vector[i] == 0) && + (knot_vector[i+k]-knot_vector[i+1] == 0)) { + return 0; + } else { + return ((u - knot_vector[i])*Nmix(i,k-1,u,knot_vector))/ + (knot_vector[i+k-1]-knot_vector[i]) + ((knot_vector[i+k] - u)* + Nmix(i+1,k-1,u,knot_vector))/(knot_vector[i+k]-knot_vector[i+1]); + } + } + else return -1; +} + + + +double Rden(double u, unsigned int k, + std::vector nurbs_control_points, + std::vector knot_vector) { + + unsigned int i; + double d = 0.0; + for (i=0; i<(nurbs_control_points.size()); i++) + d = d + Nmix(i,k,u,knot_vector)*nurbs_control_points[i].W; + return d; +} + +PLANE_POINT nurbs_point(double u, unsigned int k, + std::vector nurbs_control_points, + std::vector knot_vector) { + + unsigned int i; + PLANE_POINT point; + point.X = 0; + point.Y = 0; + for (i=0; i<(nurbs_control_points.size()); i++) { + point.X = point.X + nurbs_control_points[i].X*Nmix(i,k,u,knot_vector) + *nurbs_control_points[i].W/Rden(u,k,nurbs_control_points,knot_vector); + point.Y = point.Y + nurbs_control_points[i].Y*Nmix(i,k,u,knot_vector) + *nurbs_control_points[i].W/Rden(u,k,nurbs_control_points,knot_vector); + } + return point; +} + +double alpha_finder(double dx, double dy) { + + if ((dx > 0 && dy > 0) || (dx > 0 && dy < 0)) { /* First quadrant, Fourth quadrant */ + return atan(dy/dx) + 2*M_PI; + } else { /* Second quadrant, Third quadrant */ + return atan(dy/dx) + M_PI; + } +} diff -ruN src/emc/rs274ngc/rs274ngc.hh src/emc/rs274ngc/rs274ngc.hh --- src/emc/rs274ngc/rs274ngc.hh 2009-02-02 16:39:05.000000000 -0600 +++ src/emc/rs274ngc/rs274ngc.hh 2009-02-27 15:15:55.000000000 -0600 @@ -24,6 +24,7 @@ #include #include "canon.hh" +#include "emc.hh" typedef struct setup_struct setup; #ifndef JAVA_DIAG_APPLET @@ -184,6 +185,8 @@ int check_m_codes(block_pointer block); int check_other_codes(block_pointer block); int close_and_downcase(char *line); + int convert_nurbs(int move, block_pointer block, setup_pointer settings); + int convert_spline(int move, block_pointer block, setup_pointer settings); int comp_get_current(setup_pointer settings, double *x, double *y, double *z); int comp_set_current(setup_pointer settings, double x, double y, double z); int comp_get_programmed(setup_pointer settings, double *x, double *y, double *z); diff -ruN src/emc/sai/saicanon.cc src/emc/sai/saicanon.cc --- src/emc/sai/saicanon.cc 2009-01-29 11:56:04.000000000 -0600 +++ src/emc/sai/saicanon.cc 2009-02-27 15:12:15.000000000 -0600 @@ -432,6 +432,43 @@ /* Machining Functions */ +void NURBS_FEED( +std::vector nurbs_control_points, unsigned int k) +{ + fprintf(_outfile, "%5d ", _line_number++); + print_nc_line_number(); + fprintf(_outfile, "NURBS_FEED(%d\n", + nurbs_control_points.size()); + + _program_position_x = nurbs_control_points[nurbs_control_points.size()].X; + _program_position_y = nurbs_control_points[nurbs_control_points.size()].Y; +} + +void SPLINE_FEED( +double x1, double y1, double x2, double y2, double x3, double y3) +{ + fprintf(_outfile, "%5d ", _line_number++); + print_nc_line_number(); + fprintf(_outfile, "SPLINE_FEED(%.4f, %.4f, %.4f, %.4f, %.4f, %.4f\n", + x1, y1, x2, y2, x3, y3); + + _program_position_x = x3; + _program_position_y = y3; +} + +void SPLINE_FEED( +double x1, double y1, double x2, double y2) +{ + fprintf(_outfile, "%5d ", _line_number++); + print_nc_line_number(); + fprintf(_outfile, "SPLINE_FEED(%.4f, %.4f, %.4f, %.4f\n", + x1, y1, x2, y2); + + _program_position_x = x2; + _program_position_y = y2; +} + + void ARC_FEED(int line_number, double first_end, double second_end, double first_axis, double second_axis, int rotation, double axis_end_point diff -ruN src/emc/task/emccanon.cc src/emc/task/emccanon.cc --- src/emc/task/emccanon.cc 2009-02-20 08:25:07.000000000 -0600 +++ src/emc/task/emccanon.cc 2009-02-27 15:40:26.000000000 -0600 @@ -1139,6 +1139,195 @@ return dev; } +/* Spline and NURBS additional functions; */ + +static double max(double a, double b) { + if(a < b) return b; + return a; +} +static void unit(double *x, double *y) { + double h = hypot(*x, *y); + if(h != 0) { *x/=h; *y/=h; } +} + +static void +arc(double x0, double y0, double x1, double y1, double dx, double dy) { + double small = 0.000001; + double x = x1-x0, y=y1-y0; + double den = 2 * (y*dx - x*dy); + double r = -(x*x+y*y)/den; + double i = dy*r, j = -dx*r; + double cx = x1+i, cy=y1+j; + if (fabs(den) > small) { + ARC_FEED(interp_list.get_line_number(), x1, y1, cx, cy, r<0 ? 1 : -1, + TO_PROG_LEN(canonEndPoint.z - programOrigin.z), TO_PROG_ANG(canonEndPoint.a), + TO_PROG_ANG(canonEndPoint.b), TO_PROG_ANG(canonEndPoint.c), + TO_PROG_ANG(canonEndPoint.u),TO_PROG_ANG(canonEndPoint.v), + TO_PROG_ANG(canonEndPoint.w)); + } else { + STRAIGHT_FEED(interp_list.get_line_number(), x1,y1, + TO_PROG_LEN(canonEndPoint.z), TO_PROG_ANG(canonEndPoint.a), + TO_PROG_ANG(canonEndPoint.b), TO_PROG_ANG(canonEndPoint.c), + TO_PROG_ANG(canonEndPoint.u),TO_PROG_ANG(canonEndPoint.v), + TO_PROG_ANG(canonEndPoint.w)); + } +} + +static int +biarc(double p0x, double p0y, double tsx, double tsy, + double p4x, double p4y, double tex, double tey, double r=1.0) { + unit(&tsx, &tsy); + unit(&tex, &tey); + + double vx = p0x - p4x, vy = p0y - p4y; + double c = vx*vx + vy*vy; + double b = 2 * (vx * (r*tsx + tex) + vy * (r*tsy + tey)); + double a = 2 * r * (tsx * tex + tsy * tey - 1); + + double discr = b*b - 4*a*c; + if(discr < 0) return 0; + + double disq = sqrt(discr); + double beta1 = (-b-disq) / 2 / a; + double beta2 = (-b+disq) / 2 / a; + + if(beta1 > 0 && beta2 > 0) + return 0; + double beta = max(beta1, beta2); + double alpha = beta * r; + double ab = alpha + beta; + double p1x = p0x + alpha * tsx, p1y = p0y + alpha * tsy, + p3x = p4x - beta * tex, p3y = p4y - beta * tey, + p2x = (p1x*beta + p3x*alpha) / ab, + p2y = (p1y*beta + p3y*alpha) / ab; + double tmx = p3x-p2x, tmy = p3y-p2y; + unit(&tmx, &tmy); + + arc(p0x, p0y, p2x, p2y, tsx, tsy); + arc(p2x, p2y, p4x, p4y, tmx, tmy); + return 1; +} + + +/* Canon calls */ + +void NURBS_FEED(std::vector nurbs_control_points, unsigned int k) { + double u = 0.0; + unsigned int n = nurbs_control_points.size() - 1; + double umax = n - k + 2; + unsigned int div = nurbs_control_points.size()*4; + double dxs,dys,dx1,dy1,dx2,dy2,dxe,dye,alpha1,alpha2, alphaM; + std::vector knot_vector = knot_vector_creator(n, k); + PLANE_POINT P0, P1, P2; + + P0 = nurbs_point(u,k,nurbs_control_points,knot_vector); + P1 = nurbs_point(u+umax/div,k,nurbs_control_points,knot_vector); + + + dxs = nurbs_control_points[1].X-nurbs_control_points[0].X; + dys = nurbs_control_points[1].Y-nurbs_control_points[0].Y; + unit(&dxs,&dys); + u = u + umax/div; + while (u+umax/div <= umax) { + P2= nurbs_point(u+umax/div,k,nurbs_control_points,knot_vector); + dx1 = P1.X-P0.X; + dy1 = P1.Y-P0.Y; + dx2 = P2.X-P1.X; + dy2 = P2.Y-P1.Y; + alpha1 = alpha_finder(dx1,dy1); + alpha2 = alpha_finder(dx2,dy2); + if (alpha2 > alpha1 + M_PI) + alphaM = (alpha1+alpha2)/2 + M_PI; + else + alphaM = (alpha1+alpha2)/2; + dxe = cos(alphaM); + dye = sin(alphaM); + unit(&dxe,&dye); + biarc(P0.X,P0.Y,dxs,dys,P1.X,P1.Y,dxe,dye); + //printf("___________________________________________\n"); + //printf("X %8.4f Y %8.4f\n", P0.X, P0.Y); + dxs = dxe; + dys = dye; + P0 = P1; + P1 = P2; + //printf("u = %f\n", u); + u = u + umax/div; + } + P1.X = nurbs_control_points[n].X; + P1.Y = nurbs_control_points[n].Y; + dxe = nurbs_control_points[n].X - nurbs_control_points[n-1].X; + dye = nurbs_control_points[n].Y - nurbs_control_points[n-1].Y; + unit(&dxe,&dye); + biarc(P0.X,P0.Y,dxs,dys,P1.X,P1.Y,dxe,dye); + //printf("parameters: n = %d, umax = %f, div= %d, u = %f, k = %d\n",n,umax,div,u,k); + knot_vector.clear(); +} + +void SPLINE_FEED(double x1, double y1, double x2, double y2) { + flush_segments(); + + double x0 = TO_PROG_LEN(canonEndPoint.x); + double y0 = TO_PROG_LEN(canonEndPoint.y); + double xx0 = 2*(x1-x0), xx1 = 2*(x2-x1), + yy0 = 2*(y1-y0), yy1 = 2*(y2-y1), + ox = x0, oy = y0, odx = xx0, ody = yy0; + +#define N 2 + for(int i=1; i<=N; i++) { + double t = i * 1. / N; + double u = 1. / N; + double t0 = (1-t)*(1-t); + double t1 = 2*t*(1-t); + double t2 = t*t; + double q0 = (1-t); + double q1 = t; + +perturb: + double x = x0*t0 + x1*t1 + x2*t2; + double y = y0*t0 + y1*t1 + y2*t2; + double dx = xx0*q0 + xx1*q1; + double dy = yy0*q0 + yy1*q1; + if(!biarc(ox, oy, odx, ody, x, y, dx, dy)) { + t = t - u; u /= -2; goto perturb; + } + ox = x; oy = y; odx = dx; ody = dy; + } +} + +void SPLINE_FEED(double x1, double y1, double x2, double y2, double x3, double y3) { + flush_segments(); + + double x0 = TO_PROG_LEN(canonEndPoint.x); + double y0 = TO_PROG_LEN(canonEndPoint.y); + double xx0 = 3*(x1-x0), xx1 = 3*(x2-x1), xx2 = 3*(x3-x2), + yy0 = 3*(y1-y0), yy1 = 3*(y2-y1), yy2 = 3*(y3-y2), + ox = x0, oy = y0, odx = xx0, ody = yy0; + +//#define N 4 + for(int i=1; i<=N; i++) { + double t = i * 1. / N; + double u = 1. / N; + double t3 = t*t*t; + double t2 = 3*t*t*(1-t); + double t1 = 3*t*(1-t)*(1-t); + double t0 = (1-t)*(1-t)*(1-t); + double q0 = (1-t)*(1-t); + double q1 = 2*t*(1-t); + double q2 = t*t; + +perturb: + double x = x0*t0 + x1*t1 + x2*t2 + x3*t3; + double y = y0*t0 + y1*t1 + y2*t2 + y3*t3; + double dx = xx0*q0 + xx1*q1 + xx2*q2; + double dy = yy0*q0 + yy1*q1 + yy2*q2; + if(!biarc(ox, oy, odx, ody, x, y, dx, dy)) { + t = t - u; u /= -2; goto perturb; + } + ox = x; oy = y; odx = dx; ody = dy; + } +} + + void ARC_FEED(int line_number, double first_end, double second_end, double first_axis, double second_axis, int rotation, diff -ruN src/hal/drivers/hal_m5i20.c src/hal/drivers/hal_m5i20.c --- src/hal/drivers/hal_m5i20.c 2008-12-24 21:17:31.000000000 -0600 +++ src/hal/drivers/hal_m5i20.c 2009-02-27 16:29:41.000000000 -0600 @@ -298,7 +298,8 @@ while((i < MAX_DEVICES) && ((pDev = pci_find_subsys(M5I20_VENDOR_ID, M5I20_DEVICE_ID, M5I20_SUBSYS_VENDOR_ID, M5I20_SUBSYS_DEVICE_ID, pDev)) != NULL)){ #else - while((i < MAX_DEVICES) && ((pDev = pci_find_device(M5I20_VENDOR_ID, M5I20_DEVICE_ID, pDev)) != NULL)){ + // EBo -- trying out replacement with get... + while((i < MAX_DEVICES) && ((pDev = pci_get_device(M5I20_VENDOR_ID, M5I20_DEVICE_ID, pDev)) != NULL)){ #endif // Allocate memory for device object. diff -ruN src/hal/drivers/hal_motenc.c src/hal/drivers/hal_motenc.c --- src/hal/drivers/hal_motenc.c 2008-11-07 16:12:43.000000000 -0600 +++ src/hal/drivers/hal_motenc.c 2009-02-27 16:30:21.000000000 -0600 @@ -304,7 +304,8 @@ i = 0; // Find a MOTENC card. - while((i < MAX_DEVICES) && ((pDev = pci_find_device(MOTENC_VENDOR_ID, MOTENC_DEVICE_ID, pDev)) != NULL)){ + // EBo -- replace find with get... + while((i < MAX_DEVICES) && ((pDev = pci_get_device(MOTENC_VENDOR_ID, MOTENC_DEVICE_ID, pDev)) != NULL)){ // Allocate memory for device object. pDevice = hal_malloc(sizeof(Device)); diff -ruN src/hal/drivers/hal_vti.c src/hal/drivers/hal_vti.c --- src/hal/drivers/hal_vti.c 2008-11-14 15:41:55.000000000 -0600 +++ src/hal/drivers/hal_vti.c 2009-02-27 16:30:52.000000000 -0600 @@ -832,7 +832,8 @@ /* scans possible addresses for vti cards */ static int vti_autodetect() { - dev = pci_find_device(VENDOR, DEVICE, dev); + // EBo -- replace find with get... + dev = pci_get_device(VENDOR, DEVICE, dev); if (dev) { rtapi_print_msg(RTAPI_MSG_INFO, "VTI: Card detected in slot: %2x\n", PCI_SLOT(dev->devfn)); diff -ruN src/hal/drivers/mesa_5i2x/hal_5i2x.c src/hal/drivers/mesa_5i2x/hal_5i2x.c --- src/hal/drivers/mesa_5i2x/hal_5i2x.c 2008-04-26 20:43:44.000000000 -0500 +++ src/hal/drivers/mesa_5i2x/hal_5i2x.c 2009-02-27 16:26:42.000000000 -0600 @@ -176,7 +176,9 @@ pDev = NULL; for ( n = 0 ; n < MAX_BOARDS ; n++ ) { // Find a M5I20 card. - pDev = pci_find_device(M5I20_VENDOR_ID, M5I20_DEVICE_ID, pDev); + // EBo -- trying out replacement with get... + //pDev = pci_find_device(M5I20_VENDOR_ID, M5I20_DEVICE_ID, pDev); + pDev = pci_get_device(M5I20_VENDOR_ID, M5I20_DEVICE_ID, pDev); if ( pDev == NULL ) { /* no more boards */ break; diff -ruN src/hal/drivers/opto_ac5.c src/hal/drivers/opto_ac5.c --- src/hal/drivers/opto_ac5.c 2008-10-31 10:01:23.000000000 -0500 +++ src/hal/drivers/opto_ac5.c 2009-02-27 16:31:31.000000000 -0600 @@ -98,7 +98,8 @@ for ( n = 0 ; n < MAX_BOARDS ; n++ ) { // Find a M5I20 card. - pDev = pci_find_device(opto22_VENDOR_ID, opto22_pci_AC5_DEVICE_ID, pDev); + // EBo -- replace find with get... + pDev = pci_get_device(opto22_VENDOR_ID, opto22_pci_AC5_DEVICE_ID, pDev); if ( pDev == NULL ) { /* no more boards */break;} diff -ruN nc_files/butterfly.ngc nc_files/butterfly.ngc --- nc_files/butterfly.ngc 1969-12-31 18:00:00.000000000 -0600 +++ nc_files/butterfly.ngc 2009-02-27 23:50:09.000000000 -0600 @@ -0,0 +1,100 @@ +G0 Z30 +G0 X0 Y0 +M3 S10000 +G0 Z2 +G1 Z-5 F100 +F600 + +G5.2 X3.53 Y-1.50 P2 + X5.33 Y-11.01 P1 + X3.52 Y-24.00 P1 + X0.0 Y-29.56 P1 +G5.3 + +G5.2 X-3.52 Y-24.00 P1 + X-5.33 Y-11.01 P1 + X-3.53 Y-1.50 P2 + X0.0 Y0.0 P1 +G5.3 + +G0 Z30 +G0 X-4.30 Y-5.00 +G0 Z2 +G1 Z-5 F100 +F600 + +G5.2 X-10.78 Y2.56 P1 + X-24.12 Y7.57 P1 + X-35.39 Y-1.18 P1 + X-33.00 Y-9.76 P1 + X-25.07 Y-10.98 P1 + X-19.41 Y-18.53 P1 + X-22.02 Y-25.27 P1 + X-18.77 Y-30.08 P1 + X-12.70 Y-28.16 P2 + X-8.25 Y-23.28 P1 + X-5.65 Y-22.06 P2 + X-2.94 Y-23.50 P1 +G5.3 + +G0 Z30 +G0 X2.94 Y-23.50 +G0 Z2 +G1 Z-5 F100 +F600 + +G5.2 X5.65 Y-22.06 P2 + X8.25 Y-23.28 P1 + X12.70 Y-28.16 P2 + X18.77 Y-30.08 P1 + X22.02 Y-25.27 P1 + X19.41 Y-18.53 P1 + X25.07 Y-10.98 P1 + X33.00 Y-9.76 P1 + X35.39 Y-1.18 P1 + X24.12 Y7.57 P1 + X10.78 Y2.56 P1 + X4.30 Y-5.00 P1 +G5.3 + +G0 Z30 +G0 X2 Y-1 +G0 Z2 +G1 Z-5 F100 +F600 + +G5.2 X3.00 Y3.00 P1 L3 + X4.33 Y8.35 P1 + X5.00 Y10.50 P2 + X8.44 Y17.58 P1 + X15.32 Y16.03 P1 + X14.05 Y9.75 P1 + X10.03 Y9.40 P1 + X7.87 Y12.30 P1 + X10.56 Y15.00 P1 + X12.45 Y14.00 P1 + X12.03 Y13.00 P1 +G5.3 + +G0 Z30 +G0 X-12.03 Y13.00 +G0 Z2 +G1 Z-5 F100 +F600 + +G5.2 X-12.45 Y14.00 P1 + X-10.56 Y15.00 P1 + X-7.87 Y12.30 P1 + X-10.03 Y9.40 P1 + X-14.05 Y9.75 P1 + X-15.32 Y16.03 P1 + X-8.44 Y17.58 P1 + X-5.00 Y10.50 P2 + X-4.33 Y8.35 P1 + X-3.00 Y3.00 P1 + X-2.00 Y-1.00 P1 +G5.3 + +G0 Z30 +M5 +M2