//============================================================================// //2020.05.14 00:18 LXZ // 1、修改了部分定义 // 2、将以往获取物理轴表大小的部分全替换成插补任务对象的变量 // 3、计算加减速表大小参数修改保存至插补任务对象中 //2020.05.19 17:19 LXZ // 1、修改进给输出为比例运算后输出 // 2、修改设置轴接口为interp_set_axis,并增加比例参数 //2020.05.19 17:19 LXZ // 1、修正进给比例的速度没对应修正问题 //2020.05.26 16:34 LXZ // 1、去除没有测试的多余的函数接口 // 2、增加了插补路径接口，更新相关接口的参数，主要是增停止减速 // 3、更新了插补运算算法，现在减速度是额外设置的， // 4、整天结构调整应付连续插补功能，运行代码重写 // 5、重新实现停止方法，去除所有停止相关代码，现在是设置停止位进行进给停止输出， // 真正的停止由外部自己处理，因为连续插补结构无法实现插补减速 //2020.08.07 11:15 // 1、移植比例放大功能 // 2、增加放大进给的代码。 //2021.08.21 12:55 // 1、增加pi_round比例放大函数，带了四舍五入 // 2、改进circle_get_eigth_of_part函数，利用当前进给实现精确的相位换算，能解决椭圆变速点定位困难的问题 // 3、改进插补函数interp_begin_arc_3pt_task、interp_begin_arc_task、interp_begin_line_task等接口，增加对坐标、进给的 // 比例变换。增加了interp_begin_ellipse_task函数用于启动椭圆路径加工。 // 4、改进interp_work_arc_task、interp_work_line_task、interp_stop_arc_task、interp_stop_line_task等插补算法，通过算 // 法的改进优化实现了进给的粒子细化，以前进给为齿轮比的倒数的倍数，粒子化后为单位脉冲，能实现更平稳的停止启动以及 // 加速段的平滑度 // 5、将共享加速表独立化到虚拟轴对象中，改进加减速算法为伪S曲线（中间断为直线），实现更加合理的加减速，合理避免了速度 // 曲线的突变，更快加速到目标速度，但是会增加额外的内存，目前默认大小为1K // 6、圆弧算法被替换成了椭圆算法，从而兼容圆弧与椭圆的路径加工 // 7、后级进给系统从当前的齿轮比倒数倍数还原成了以前的了1:1，并取消了下一级进给缓冲，因为现在后级倍率已经被取消，这 // 部分不需要了 //2021.08.25 00:12 // 1、增加了椭圆开方结果做四舍五入，可以减少X\Y圆弧查补因为齿轮比差异太大产生一些误判， // 2、稍微平滑了加速度表 // 3、取消了连续插补路径的位置补偿，因为现在不再是1:1的比列了，引入齿轮比后，路径会有比例变化，这时候采用这种方式去 // 计算位置修正，本身也是会带来误差的，还不如不做修正 //2022.04.11 10:47 LXZ // 通过优化8分圆判断算法，达到让椭圆弧加工过程不会速度剧烈波动造成机架抖动 // 1、增加ellipse_get_eight_of_part_by_xaxis与ellipse_get_eight_of_part_by_yaxis两个判断8分圆位置的函数 // X轴为长轴时用ellipse_get_eight_of_part_by_xaxis，Y轴为长轴时用ellipse_get_eight_of_part_by_yaxis // 删除以前判断圆8分圆位置的函数，该函数不再使用，以前使用的地方用上面两个函数代替 // 2、增加interp_calc_arc_refrence，该函数是内部调用的，用于计算圆弧的8分圆参考位置（绝对值） // 3、修改加速过程为直线加减速 //============================================================================// #include "axis_interp.h" #include #include #include #include #define M_PI 3.14159265358979323846 /** * 对数值进行比例放大后执行四舍五入 * * @author LXZ (032620) * * @param interp 插补对象 * @param start_point 开始坐标 * @param end_point 终点坐标 * @param speed 速度 */ static int pi_round(float value, float ratio) { if (value > 0) { return (int)((value * ratio) + 0.5); } else { return (int)((value * ratio) - 0.5); } } /** * 初始化插补对象的参数值 * * @author LXZ (033120) * * @param interp 插补对象 * @param ref_clcok 参考时钟 */ void interp_init(interp_task_t *interp, int ref_clcok) { int i = 0; memset(interp, 0, sizeof(interp_task_t)); for (i = 0; i < INTERP_AXIS_NUMBER; i++) { interp->axis[i].id = i; } interp->ref_clock = ref_clcok * INTERP_PERIOD; } /** * 关闭插补轴，关闭后轴将不参与计算与插补运动，如果是X轴或者Y轴，圆弧插补会强制打开 * * @author LXZ (032620) * * @param interp 插补对象 * @param axis 轴编号 */ void interp_axis_enable(interp_task_t *interp, char axis_id) { interp->axis[axis_id].flg = 1; } /** * 关闭插补轴，关闭后轴将不参与计算与插补运动，如果是X轴或者Y轴，圆弧插补会强制打开 * * @author LXZ (032620) * * @param interp 插补对象 * @param axis 轴编号 */ void interp_axis_disable(interp_task_t *interp, char axis_id) { interp->axis[axis_id].flg = 0; } /** * 为指定插补轴设置物理轴对象 * * @author LXZ (032620) * * @param interp 插补对象 * @param axis_id 轴编号 * @param axis 轴对象 * @param ratio 输出比例 */ void interp_set_axis(interp_task_t *interp, char axis_id, void *axis, float ratio, float max_acc) { interp->axis[axis_id].axis = axis; interp->axis[axis_id].ratio = ratio; interp->axis[axis_id].max_acc = max_acc; if (interp->axis[axis_id].max_acc > interp->axis[axis_id].ratio) { interp->axis[axis_id].max_acc = interp->axis[axis_id].ratio; } } /** * 对插补参数预初始化 * * @author LXZ (032620) * * @param interp 插补对象 */ static void interp_task_init(interp_task_t *interp, int speed, int stop_speed) { int i = 0; float dst_speed = 0; int max_speed; int speed_table_size = 0; int axis_stop_speed = 0; //基本参数赋值 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { //interp->axis[i].last_period = PERIOD_MAX_VALUE; //interp->axis[i].cur_speed = 0; interp->axis[i].dec_speed = 0; interp->axis[i].dec_position = 0; interp->axis[i].cur_delta = interp->axis[i].total_delta; if (interp->axis[i].flg) { interp_axis_calc_speed(&(interp->axis[i]), speed); } interp->axis[i].last_feed = 0; //关键参考坐标赋值 interp->axis[i].cur_posi = interp->start_point.vector[i]; interp->axis[i].target_posi = interp->end_point.vector[i]; interp->axis[i].real_posi = interp->start_point.vector[i]; interp->axis[i].start_posi = interp->start_point.vector[i]; //轴真实位置初始值 dst_speed = 0; max_speed = (int)(speed * interp->axis[i].ratio) / 1000; axis_stop_speed = (int)(stop_speed * interp->axis[i].ratio) / 1000; if (max_speed == 0) max_speed = 1; speed_table_size = 0; interp->axis[i].stop_speed = 0; while (dst_speed < max_speed) { dst_speed += 1; interp->axis[i].speed_table[speed_table_size] = (int)dst_speed; if (dst_speed <= axis_stop_speed) { interp->axis[i].stop_speed = speed_table_size; } speed_table_size++; } interp->axis[i].max_speed = speed_table_size; } interp->last_quadrant = 0; } /** * 复位插补任务，包括路径参数为0.开始位置为0 * * @author LXZ (032620) * * @param interp 插补对象 */ void interp_task_reset(interp_task_t *interp) { int i = 0; //缓冲清0 interp->cur_path_index = 0; interp->path_count = 0; //当前虚拟轴状态复位 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { interp->axis[i].cur_posi = 0; interp->axis[i].cur_speed = 0; interp->axis[i].real_posi = 0; interp->axis[i].axis_position = 0; interp->axis[i].cur_delta = 0; interp->axis[i].last_period = PERIOD_MAX_VALUE; interp->axis[i].dec_speed = 0; interp->axis[i].dec_position = 0; interp->axis[i].last_feed = 0; } interp->last_quadrant = 0; memset(&interp->paths[0], 0, sizeof(interp->paths[0])); //工作缓冲状态复位 interp->cur_buffer_index = 0; interp->feed_count = 0; for (i = 0; i < INTERP_WORK_BUFFER_NUMBER; i++) { interp->work_buffer[i].count = 0; } interp->stop_flg = 0; //关闭停止位，这时候周期进给任务会使能 } /** * 算当前坐标在圆弧中所处的象限 * * @author LXZ (032620) * * @param sn 圆弧方向 * @param x 相对圆心的X轴坐标值 * @param y 相对圆心的Y轴坐标值 * * @return int 象限1~4 */ static int arc_get_quadrant(int sn, int x, int y) { int quadrant = 0; if ((x > 0) && (y > 0)) quadrant = 1; else if ((x < 0) && (y > 0)) quadrant = 2; else if ((x < 0) && (y < 0)) quadrant = 3; else if ((x > 0) && (y < 0)) quadrant = 4; // 如果在坐标轴上, 按顺序 判定 if (sn == 0) { // 逆圆 if ((x > 0) && (y == 0)) quadrant = 1; else if ((x == 0) && (y > 0)) quadrant = 2; else if ((x < 0) && (y == 0)) quadrant = 3; else if ((x == 0) && (y < 0)) quadrant = 4; } else { // 1 顺圆 if ((x == 0) && (y > 0)) quadrant = 1; else if ((x < 0) && (y == 0)) quadrant = 2; else if ((x == 0) && (y < 0)) quadrant = 3; else if ((x > 0) && (y == 0)) quadrant = 4; } // 第几象限 return quadrant; } /** * 为了不经过角度计算而开发的识别椭圆8分圆区域扩展(X轴) * * @author LXZ (040922) * * @param sn 方向 * @param x 相对于圆心的X轴坐标 * @param y 相对于圆心的Y轴坐标 * @param ratio x，y轴比列 * @param last_quadrant 上一次的象限，防止象限来回跳 * * @return int * 1~8分别代码8分圆8个象限，跟四象限一样，每个象限再细分两半 */ static int ellipse_get_eight_of_part_by_xaxis(int sn, int x, int y, int refrence, int last) { int quadrant = 0; if ((x > 0) && (y > 0)) quadrant = 1; else if ((x < 0) && (y > 0)) quadrant = 2; else if ((x < 0) && (y < 0)) quadrant = 3; else if ((x > 0) && (y < 0)) quadrant = 4; else if (x == 0) { if (sn == 0) // 逆圆 { if (y > 0)quadrant = 2; else quadrant = 4; } else //顺圆 { if (y > 0) quadrant = 1; else quadrant = 3; } } else if (y == 0) { if (sn == 0) //逆圆 { if (x > 0) quadrant = 1; else quadrant = 3; } else //顺圆4 { if (x > 0) quadrant = 4; else quadrant = 2; } } if (x < 0) x = -x; //求得8分圆像限位置 switch (quadrant) { case 1: if (sn == 0) //逆圆 { if (x >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 2) quadrant = 1; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 3) quadrant = 2; else quadrant = last; } } else //顺圆 { if (x <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 1) quadrant = 2; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 2) quadrant = 3; else quadrant = last; } } break; case 2: if (sn == 0) //逆圆 { if (x >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 5) quadrant = 4; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 4) quadrant = 3; else quadrant = last; } } else //顺圆 { if (x <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 2) quadrant = 3; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 3) quadrant = 4; else quadrant = last; } } break; case 3: if (sn == 0) //逆圆 { if (x >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 6) quadrant = 5; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 7) quadrant = 6; else quadrant = last; } } else //顺圆 { if (x <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 5) quadrant = 6; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 4) quadrant = 5; else quadrant = last; } } break; case 4: if (sn == 0) //逆圆 { if (x >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 1) quadrant = 8; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 8) quadrant = 7; else quadrant = last; } } else //顺圆 { if (x <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 6) quadrant = 7; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 7) quadrant = 8; else quadrant = last; } } break; } return quadrant; } /** * 为了不经过角度计算而开发的识别椭圆8分圆区域扩展(Y轴) * * @author LXZ (040922) * * @param sn 方向 * @param x 相对于圆心的X轴坐标 * @param y 相对于圆心的Y轴坐标 * @param ratio x，y轴比列 * @param last_quadrant 上一次的象限，防止象限来回跳 * * @return int * 1~8分别代码8分圆8个象限，跟四象限一样，每个象限再细分两半 */ static int ellipse_get_eight_of_part_by_yaxis(int sn, int x, int y, int refrence,int last) { int quadrant = 0; if ((x > 0) && (y > 0)) quadrant = 1; else if ((x < 0) && (y > 0)) quadrant = 2; else if ((x < 0) && (y < 0)) quadrant = 3; else if ((x > 0) && (y < 0)) quadrant = 4; else if (x == 0) { if (sn == 0) // 逆圆 { if (y > 0)quadrant = 2; else quadrant = 4; } else //顺圆 { if (y > 0) quadrant = 1; else quadrant = 3; } } else if (y == 0) { if (sn == 0) //逆圆 { if (x > 0) quadrant = 1; else quadrant = 3; } else //顺圆4 { if (x > 0) quadrant = 4; else quadrant = 2; } } if (y < 0) y = -y; //求得8分圆像限位置 switch (quadrant) { case 1: if (sn == 0) //逆圆 { if (y >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 3) quadrant = 2; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 2) quadrant = 1; else quadrant = last; } } else //顺圆 { if (y <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 8) quadrant = 1; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 1) quadrant = 2; else quadrant = last; } } break; case 2: if (sn == 0) //逆圆 { if (y >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 4) quadrant = 3; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 5) quadrant = 4; else quadrant = last; } } else //顺圆 { if (y <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 3) quadrant = 4; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 2) quadrant = 3; else quadrant = last; } } break; case 3: if (sn == 0) //逆圆 { if (y >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 7) quadrant = 6; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 6) quadrant = 5; else quadrant = last; } } else //顺圆 { if (y <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 4) quadrant = 5; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 5) quadrant = 6; else quadrant = last; } } break; case 4: if (sn == 0) //逆圆 { if (y >= refrence) { //第二象限在逆圆情况下不可能2转向1 if (last != 8) quadrant = 7; else quadrant = last; } else { //第二象限在逆圆情况下不可能3转向2 if (last != 1) quadrant = 8; else quadrant = last; } } else //顺圆 { if (y <= refrence) { //第二象限在顺圆情况下不可能1转向2 if (last != 7) quadrant = 8; else quadrant = last; } else { //第二象限在顺圆情况下不可能2转向3 if (last != 6) quadrant = 7; else quadrant = last; } } break; } return quadrant; } /** * 选择要进行圆弧插补的有效轴 * * @author LXZ (040120) * * @param interp 插补对象 * @param axis_x_id X轴编号 * @param axis_y_id Y轴编号 */ void interp_set_arc_axis(interp_task_t *interp, char axis_x_idx, char axis_y_idx) { interp->arc_x_idx = axis_x_idx; interp->arc_y_idx = axis_y_idx; } /** * 求出两个值的平方差的开方 * * @author LXZ (032820) * * @param x 圆半径 * @param y X或者Y坐标相对圆心的偏移量 * @param ratio1 相对于输入轴的齿轮比 * @param ratio2 相对于输出轴的齿轮比 * * @return int */ int circle_sqrt(int x, int y, float ratio1, float ratio2) { float fx, fy; fx = x; fy = y / ratio1; if (fx <= fy) return 0; return (int)(sqrtf((fx * fx - fy * fy)) * ratio2); } /** * 知道中心点在0点的椭圆的一个点一个坐标值，求另一个坐标值 * * @author LXZ (032820) * * @param x 其中一个坐标值 * @param a 坐标值对应的轴长 * @param b 另一个轴的轴长 * * @return int 返回另一个轴的坐标值 */ int eclipse_sqrt(float x, float a, float b) { //float y = 0; //float r = b * b; //float res = (x * x) /(a * a) * r; //y = sqrtf(r - res); //return (int)y; return (int)(sqrtf(b * b - (x * x) / (a * a) * (b * b)) + 0.5); } /** * 增加直线路径 * * @author LXZ (032620) * * @param interp 插补对象 * @param end_point 终点坐标 * @param target_speed 目标速度 * @param stop_speed 停止速度 */ void interp_add_liner_path(interp_task_t *interp, axis_vector_t *end_point, int target_speed, int stop_speed ) { if (interp->path_count < INTERP_PATH_SIZE) { //限制路径条数 interp_path_t *path = &interp->paths[interp->path_count]; path->work_type = INTERP_MODE_LINE; //直线插补 path->end_point = *end_point; path->target_speed = target_speed; path->stop_speed = stop_speed; interp->path_count++; } } /** * 增加圆弧路径 * * @author LXZ (032620) * * @param interp 插补对象 * @param end_point 终点坐标 * @param target_speed 目标速度 * @param stop_speed 停止速度 */ void interp_add_arc_path(interp_task_t *interp, char arc_x_id, char arc_y_id, axis_vector_t *end_point, axis_vector_t *centre_point, int dir, int target_speed, int stop_speed ) { if (interp->path_count < INTERP_PATH_SIZE) { //限制路径条数 interp_path_t *path = &interp->paths[interp->path_count]; path->work_type = INTERP_MODE_ARC; //圆弧插补 path->wor_dir = dir; path->end_point = *end_point; path->centre_point = *centre_point; path->target_speed = target_speed; path->stop_speed = stop_speed; path->arc_x_id = arc_x_id; path->arc_y_id = arc_y_id; interp->path_count++; } } /** * 增加椭圆弧路径 * * @author LXZ (032620) * * @param interp 插补对象 * @param end_point 终点坐标 * @param target_speed 目标速度 * @param stop_speed 停止速度 */ void interp_add_ellipse_path(interp_task_t *interp, char arc_x_id, char arc_y_id, axis_vector_t *end_point, axis_vector_t *centre_point, int a, int b, int dir, int target_speed, int stop_speed ) { if (interp->path_count < INTERP_PATH_SIZE) { //限制路径条数 interp_path_t *path = &interp->paths[interp->path_count]; path->work_type = INTERP_MODE_ELLIPSE; //椭圆弧插补 path->wor_dir = dir; path->end_point = *end_point; path->centre_point = *centre_point; path->target_speed = target_speed; path->stop_speed = stop_speed; path->arc_x_id = arc_x_id; path->arc_y_id = arc_y_id; path->radius_x = a; path->radius_y = b; interp->path_count++; } } /** * 计算圆弧的8分之一参照位置 * * @author LXZ (041022) * * @param interp 插补对象 */ static void interp_calc_arc_refrence(interp_task_t* interp) { //计算原理 //要到达8分之一位置 ，需要有X轴走的比例与Y轴走的比例一样 //假设椭圆长轴的轴半径是A,短轴的轴半径是B //即在椭圆中长轴走的距离a与短轴走的距离b关系是，则b = a * B / A; //由于椭圆的直线方程为 (X^2)/(A^2)+(Y^2)/(B^2) = 1 //由于参照点是在椭圆上的，会满足椭圆方程，那么Y=b= a*B/A=X*B/A; //则方程可以变化为 //(X^2)/(A^2) + ((X*B/A)^2)/(B^2) = 1 //所以求X点坐标可以这么求 //X =sqrt(1/(1/(A^2) + ((B/A)^2)/B^2))) = sqrt(1/(1/(A^2) + 1/(A^2))) = sqrt((A^2)/2) = A* sqrt(0.5); interp->quadrant_refrence.vector[interp->arc_x_idx] = interp->axis_length.vector[interp->arc_x_idx] * sqrt(0.5); interp->quadrant_refrence.vector[interp->arc_y_idx] = interp->axis_length.vector[interp->arc_y_idx] * sqrt(0.5); } /** * 通过三点求圆弧的方式，开始插补任务，不满足圆弧时利用直线去走 * * @author LXZ (033120) * * @param interp 插补对象 * @param start_point 开始坐标 * @param end_point 终点坐标 * @param middle_point 中点坐标 * @param speed 速度 */ void interp_begin_arc_3pt_task( interp_task_t *interp, axis_vector_t *start_point, axis_vector_t *end_point, axis_vector_t *middle_point, int speed ) { int i = 0; float ax, ay, bx, by, cx, cy; float line_flg = 0; // 1, 三点共线 float mid_a, mid_b, mid_c, mid_d; float mid_e, mid_f; float x11_org, y11_org; float radius; ax = start_point->vector[interp->arc_x_idx]; ay = start_point->vector[interp->arc_y_idx]; bx = middle_point->vector[interp->arc_x_idx]; by = middle_point->vector[interp->arc_y_idx]; cx = end_point->vector[interp->arc_x_idx]; cy = end_point->vector[interp->arc_y_idx]; //三点重合，不符合直线 if (((ax == bx) && (cx == bx)) && ((by == ay) && (cy == by))) { interp->mode = INTERP_MODE_FREE; return; } //计算斜率是否一样 //由公式(y2 - y1)/(x2 - x1) = (y3 - y2) / (x3 - x2) //可得变形式 (y2 - y1)* (x3 - x2) - (y3- y2) *(x2 - x1) = 0 //结果不为0说明不是一条直线，但是可以省掉除法从而提高正确率 line_flg = (bx - ax) * (cy - by) - (cx - bx) * (by - ay); if (line_flg > 0) { //该圆为逆时针的圆，因为斜率增大 interp->arc_dir = 0; } else if (line_flg < 0) { //该圆为顺圆 interp->arc_dir = 1; } else { //这是直线，必须用直线方式运动 interp_begin_line_task(interp, start_point, end_point, speed, 0); return; } //计算圆心坐标与半径 //中间计算公式 mid_a = ax - bx; mid_b = ay - by; mid_c = ax - cx; mid_d = ay - cy; mid_e = ((ax * ax - bx * bx) - (by * by - ay * ay)) / 2.0f; mid_f = ((ax * ax - cx * cx) - (cy * cy - ay * ay)) / 2.0f; //计算(ax - cx) * (ay - by) - (ax - bx) * (ay - cy) != 0 //同于计算if((ay - cy)/(ax - cx) != (ay - by) / (ax - bx)) //这个其实是判断三个点的斜率是否有变化，没有任何变化，说明三个点是在一个线上，不可能构成圆 //通过将除法运算变换成乘法，可以规避除数为0不合法导致异常的事情发生（求斜率经常发生的事） if ((mid_b * mid_c - mid_a * mid_d) != 0) { //通过圆的方程x*x + y*y = r*r //可以分别获得点[a,b]与[a,c]对于圆心(x0,y0)的方程 //经过不同变换化去x0或者y0可以求出另一个值，公式太麻烦不列出，但是完全可以从圆方程推导出来 x11_org = -(mid_d * mid_e - mid_b * mid_f) / (mid_b * mid_c - mid_a * mid_d); y11_org = -(mid_a * mid_f - mid_c * mid_e) / (mid_b * mid_c - mid_a * mid_d); } else { interp_begin_line_task(interp, start_point, end_point, speed, 0); return; } //限制有效值是小数点后三位,如果输入是直接脉冲当量，就不例会了 //if (Math.Abs(x11_org) < 1.0e-3) x11_org = 0.0f; //if (Math.Abs(y11_org) < 1.0e-3) y11_org = 0.0f; //圆弧需要强制开启X轴与Y轴,暂时不支持Z轴同时插补，所以会关闭Z轴 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { if (i == interp->arc_x_idx || i == interp->arc_y_idx) { interp_axis_enable(interp, i); } else { interp_axis_disable(interp, i); } } interp->mode = INTERP_MODE_ARC; interp->start_point = *start_point; interp->end_point = *end_point; //计算半径 radius = (float)sqrtf((x11_org - ax) * (x11_org - ax) + (y11_org - ay) * (y11_org - ay)); //保存圆心坐标 interp->centre_point.vector[interp->arc_x_idx] = (int)x11_org; interp->centre_point.vector[interp->arc_y_idx] = (int)y11_org; interp->arc_radius = (int)radius; //起点与终点所在的象限 int quadrant_start = arc_get_quadrant(interp->arc_dir, (int)(ax - interp->centre_point.vector[interp->arc_x_idx]), (int)(ay - interp->centre_point.vector[interp->arc_y_idx])); int quadrant_end = arc_get_quadrant(interp->arc_dir, (int)(cx - interp->centre_point.vector[interp->arc_x_idx]), (int)(cy - interp->centre_point.vector[interp->arc_y_idx])); //计算两轴的总进给 ax = interp->start_point.vector[interp->arc_x_idx] - interp->centre_point.vector[interp->arc_x_idx]; ay = interp->start_point.vector[interp->arc_y_idx] - interp->centre_point.vector[interp->arc_y_idx]; cx = interp->end_point.vector[interp->arc_x_idx] - interp->centre_point.vector[interp->arc_x_idx]; cy = interp->end_point.vector[interp->arc_y_idx] - interp->centre_point.vector[interp->arc_y_idx]; { //求角度， float start_deg = (float)(acos((float)ax / interp->arc_radius) * 180 / M_PI); float end_deg = (float)(acos((float)cx / interp->arc_radius) * 180 / M_PI); float delta_deg = 0; if (quadrant_start == 1 && start_deg == 0 && interp->arc_dir == 1) //当判断是第一象限并且是顺针同时也是角度0的时候， // 其实它是算第四象限才对 { start_deg = 360; } else if (quadrant_start > 2) { //180度修正 start_deg = 360 - start_deg; } if ((quadrant_end == 1 && end_deg == 0 && interp->arc_dir == 0)) //当判断是第一象限并且是逆时针同时也是角度0的时候， // 其实它是算第四象限才对 { end_deg = 360; } else if (quadrant_end > 2) { end_deg = 360 - end_deg; } //初始化总进给量 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { interp->axis[i].total_delta = 0; } //求总进给角度 //根据角度求出具体的总进给值 if (interp->arc_dir == 0) { //逆时针 delta_deg = end_deg - start_deg; if (delta_deg < 0) { delta_deg += 360; } if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx))); } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx))); } else if (start_deg >= end_deg) { //大于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax)))); } else { //小于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax)); } if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy))); } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy))); } else if (delta_deg > 180) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay)))); } else { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay)); } } else { //顺时针 delta_deg = start_deg - end_deg; if (delta_deg < 0) { delta_deg += 360; } if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx))); } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx))); } else if (start_deg <= end_deg) { //大于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax)))); } else { //小于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax)); } if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy))); } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy))); } else if (delta_deg > 180) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay)))); } else { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay)); } } } //进给量比例变换 interp->axis[interp->arc_x_idx].total_delta = pi_round(interp->axis[interp->arc_x_idx].total_delta, interp->axis[interp->arc_x_idx].ratio); interp->axis[interp->arc_y_idx].total_delta = pi_round(interp->axis[interp->arc_y_idx].total_delta, interp->axis[interp->arc_y_idx].ratio); interp->start_point.vector[interp->arc_x_idx] = pi_round(interp->start_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->start_point.vector[interp->arc_y_idx] = pi_round(interp->start_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->end_point.vector[interp->arc_x_idx] = pi_round(interp->end_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->end_point.vector[interp->arc_y_idx] = pi_round(interp->end_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->centre_point.vector[interp->arc_x_idx] = pi_round(interp->centre_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->centre_point.vector[interp->arc_y_idx] = pi_round(interp->centre_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->axis_length.vector[interp->arc_x_idx] = pi_round(interp->arc_radius, interp->axis[interp->arc_x_idx].ratio); interp->axis_length.vector[interp->arc_y_idx] = pi_round(interp->arc_radius, interp->axis[interp->arc_y_idx].ratio); //计算8分圆的参考位置（相对圩圆心的绝对偏移） interp_calc_arc_refrence(interp); //利用三点一计算出圆心与方向 interp_task_init(interp, speed, 0); } /** * 通过圆心与方向，开始圆弧的插补任务 * * @author LXZ (032620) * * @param interp 插补对象 * @param start_point 开始坐标 * @param end_point 终点坐标 * @param centre_point 圆心坐标 * @param dir 方向，0表示逆时针，1表示顺时针 */ void interp_begin_arc_task( interp_task_t *interp, axis_vector_t *start_point, axis_vector_t *end_point, axis_vector_t *centre_point, int dir, int speed, int stop_speed ) { float dx, dy, radius; int ax, ay, cx, cy; int i = 0; //求半径 dx = centre_point->vector[interp->arc_x_idx] - start_point->vector[interp->arc_x_idx]; dy = centre_point->vector[interp->arc_y_idx] - start_point->vector[interp->arc_y_idx]; ax = start_point->vector[interp->arc_x_idx] - centre_point->vector[interp->arc_x_idx]; ay = start_point->vector[interp->arc_y_idx] - centre_point->vector[interp->arc_y_idx]; cx = end_point->vector[interp->arc_x_idx] - centre_point->vector[interp->arc_x_idx]; cy = end_point->vector[interp->arc_y_idx] - centre_point->vector[interp->arc_y_idx]; radius = sqrtf(dx * dx + dy * dy); //初始值 interp->arc_dir = dir; interp->centre_point = *centre_point; interp->start_point = *start_point; interp->end_point = *end_point; interp->arc_radius = (int)radius; //计算进给总量 //圆弧需要强制开启X轴与Y轴,暂时不支持Z轴同时插补，所以会关闭Z轴 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { if (i == interp->arc_x_idx || i == interp->arc_y_idx) { interp_axis_enable(interp, i); } else { interp_axis_disable(interp, i); } } interp->mode = INTERP_MODE_ARC; if (start_point->vector[interp->arc_x_idx] == end_point->vector[interp->arc_x_idx] && start_point->vector[interp->arc_y_idx] == end_point->vector[interp->arc_y_idx]) { interp->axis[interp->arc_x_idx].total_delta = interp->arc_radius * 4; interp->axis[interp->arc_y_idx].total_delta = interp->arc_radius * 4; } else { //起点与终点所在的象限 int quadrant_start = arc_get_quadrant(interp->arc_dir, ax, ay); int quadrant_end = arc_get_quadrant(interp->arc_dir, cx, cy); //求角度， float start_deg = (float)(acosf((float)ax / interp->arc_radius) * 180 / M_PI); float end_deg = (float)(acosf((float)cx / interp->arc_radius) * 180 / M_PI); float delta_deg = 0; //根据象限修正角度 if (quadrant_start == 1 && start_deg == 0 && interp->arc_dir == 1) //当判断是第一象限并且是顺针同时也是角度0的时候， // 其实它是算第四象限才对 { start_deg = 360; } else if (quadrant_start > 2) { //180度修正 start_deg = 360 - start_deg; } if ((quadrant_end == 1 && end_deg == 0 && interp->arc_dir == 0)) //当判断是第一象限并且是逆时针同时也是角度0的时候， // 其实它是算第四象限才对 { end_deg = 360; } else if (quadrant_end > 2) { end_deg = 360 - end_deg; } for (i = 0; i < INTERP_AXIS_NUMBER; i++) { interp->axis[i].total_delta = 0; } //根据角度求进给量 //根据角度求出具体的总进给值 //根据角度求出具体的总进给值 if (interp->arc_dir == 0) { //逆时针 delta_deg = end_deg - start_deg; if (delta_deg < 0) { delta_deg += 360; } if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx))); } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx))); } else if (start_deg >= end_deg) { //大于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax)))); } else { //小于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax)); } if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy))); } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy))); } else if (delta_deg > 180) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay)))); } else { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay)); } } else { //顺时针 delta_deg = start_deg - end_deg; if (delta_deg < 0) { delta_deg += 360; } if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius - ax) + (interp->arc_radius - cx))); } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((interp->arc_radius + ax) + (interp->arc_radius + cx))); } else if (start_deg <= end_deg) { //大于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(interp->arc_radius * 4 - abs((int)(cx - ax)))); } else { //小于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax)); } if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius + ay) + (interp->arc_radius + cy))); } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((interp->arc_radius - ay) + (interp->arc_radius - cy))); } else if (delta_deg > 180) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * interp->arc_radius - abs((int)(cy - ay)))); } else { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay)); } } } //进给量比例变换 interp->axis[interp->arc_x_idx].total_delta = pi_round(interp->axis[interp->arc_x_idx].total_delta, interp->axis[interp->arc_x_idx].ratio); interp->axis[interp->arc_y_idx].total_delta = pi_round(interp->axis[interp->arc_y_idx].total_delta, interp->axis[interp->arc_y_idx].ratio); interp->start_point.vector[interp->arc_x_idx] = pi_round(interp->start_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->start_point.vector[interp->arc_y_idx] = pi_round(interp->start_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->end_point.vector[interp->arc_x_idx] = pi_round(interp->end_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->end_point.vector[interp->arc_y_idx] = pi_round(interp->end_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->centre_point.vector[interp->arc_x_idx] = pi_round(interp->centre_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->centre_point.vector[interp->arc_y_idx] = pi_round(interp->centre_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->axis_length.vector[interp->arc_x_idx] = (int)pi_round(radius, interp->axis[interp->arc_x_idx].ratio); interp->axis_length.vector[interp->arc_y_idx] = (int)pi_round(radius, interp->axis[interp->arc_y_idx].ratio); //计算8分圆的参考位置（相对圩圆心的绝对偏移） interp_calc_arc_refrence(interp); interp_task_init(interp, speed, stop_speed); } /** * 通过椭圆圆心与方向，开始椭圆弧的插补任务 * * @author LXZ (032620) * * @param interp 插补对象 * @param start_point 开始坐标 * @param end_point 终点坐标 * @param centre_point 圆心坐标 * @param radius_x 椭圆弧X轴轴长 * @param radius_y 椭圆弧Y轴轴长 * @param dir 方向，0表示逆时针，1表示顺时针 */ void interp_begin_ellipse_task( interp_task_t *interp, axis_vector_t *start_point, axis_vector_t *end_point, axis_vector_t *centre_point, float radius_x, float radius_y, int dir, int speed, int stop_speed) { int ax, ay, cx, cy; int i = 0; //求半径 //dx = centre_point->vector[interp->arc_x_idx] - // start_point->vector[interp->arc_x_idx]; //dy = centre_point->vector[interp->arc_y_idx] - // start_point->vector[interp->arc_y_idx]; ax = start_point->vector[interp->arc_x_idx] - centre_point->vector[interp->arc_x_idx]; ay = start_point->vector[interp->arc_y_idx] - centre_point->vector[interp->arc_y_idx]; cx = end_point->vector[interp->arc_x_idx] - centre_point->vector[interp->arc_x_idx]; cy = end_point->vector[interp->arc_y_idx] - centre_point->vector[interp->arc_y_idx]; //初始值 interp->arc_dir = dir; interp->centre_point = *centre_point; interp->start_point = *start_point; interp->end_point = *end_point; //计算进给总量 //圆弧需要强制开启X轴与Y轴,暂时不支持Z轴同时插补，所以会关闭Z轴 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { if (i == interp->arc_x_idx || i == interp->arc_y_idx) { interp_axis_enable(interp, i); } else { interp_axis_disable(interp, i); } } interp->mode = INTERP_MODE_ARC; if (start_point->vector[interp->arc_x_idx] == end_point->vector[interp->arc_x_idx] && start_point->vector[interp->arc_y_idx] == end_point->vector[interp->arc_y_idx]) { interp->axis[interp->arc_x_idx].total_delta = (int)(radius_x * 4); interp->axis[interp->arc_y_idx].total_delta = (int)(radius_y * 4); } else { //起点与终点所在的象限 int quadrant_start = arc_get_quadrant(interp->arc_dir, ax, ay); int quadrant_end = arc_get_quadrant(interp->arc_dir, cx, cy); //求角度， float start_deg = (float)(acosf((float)ax / radius_x) * 180 / M_PI); float end_deg = (float)(acosf((float)cx / radius_y) * 180 / M_PI); float delta_deg = 0; //根据象限修正角度 if (quadrant_start == 1 && start_deg == 0 && interp->arc_dir == 1) //当判断是第一象限并且是顺针同时也是角度0的时候， // 其实它是算第四象限才对 { start_deg = 360; } else if (quadrant_start > 2) { //180度修正 start_deg = 360 - start_deg; } if ((quadrant_end == 1 && end_deg == 0 && interp->arc_dir == 0)) //当判断是第一象限并且是逆时针同时也是角度0的时候， // 其实它是算第四象限才对 { end_deg = 360; } else if (quadrant_end > 2) { end_deg = 360 - end_deg; } for (i = 0; i < INTERP_AXIS_NUMBER; i++) { interp->axis[i].total_delta = 0; } //根据角度求进给量 //根据角度求出具体的总进给值 //根据角度求出具体的总进给值 if (interp->arc_dir == 0) { //逆时针 delta_deg = end_deg - start_deg; if (delta_deg < 0) { delta_deg += 360; } if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x + ax) + (radius_x + cx))); } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x - ax) + (radius_x - cx))); } else if (start_deg >= end_deg) { //大于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(radius_x * 4 - abs((int)(cx - ax)))); } else { //小于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax)); } if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y - ay) + (radius_y - cy))); } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y + ay) + (radius_y + cy))); } else if (delta_deg > 180) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * radius_y - abs((int)(cy - ay)))); } else { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay)); } } else { //顺时针 delta_deg = start_deg - end_deg; if (delta_deg < 0) { delta_deg += 360; } if (start_deg <= 180 && end_deg >= 180) { //从圆左边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x - ax) + (radius_x - cx))); } else if (start_deg >= 180 && end_deg <= 180) { //从圆右边跨象限 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)((radius_x + ax) + (radius_x + cx))); } else if (start_deg <= end_deg) { //大于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(radius_x * 4 - abs((int)(cx - ax)))); } else { //小于半圆 interp->axis[interp->arc_x_idx].total_delta = (int)abs((int)(cx - ax)); } if ((start_deg <= 90 || start_deg >= 270) && (end_deg <= 270 && end_deg >= 90)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y + ay) + (radius_y + cy))); } else if ((start_deg >= 90 && start_deg <= 270) && (end_deg <= 90 || end_deg >= 270)) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)((radius_y - ay) + (radius_y - cy))); } else if (delta_deg > 180) { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(4 * radius_y - abs((int)(cy - ay)))); } else { interp->axis[interp->arc_y_idx].total_delta = (int)abs((int)(cy - ay)); } } } //进给量比例变换 interp->axis[interp->arc_x_idx].total_delta = pi_round(interp->axis[interp->arc_x_idx].total_delta, interp->axis[interp->arc_x_idx].ratio); interp->axis[interp->arc_y_idx].total_delta = pi_round(interp->axis[interp->arc_y_idx].total_delta, interp->axis[interp->arc_y_idx].ratio); interp->start_point.vector[interp->arc_x_idx] = pi_round(interp->start_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->start_point.vector[interp->arc_y_idx] = pi_round(interp->start_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->end_point.vector[interp->arc_x_idx] = pi_round(interp->end_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->end_point.vector[interp->arc_y_idx] = pi_round(interp->end_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->centre_point.vector[interp->arc_x_idx] = pi_round(interp->centre_point.vector[interp->arc_x_idx], interp->axis[interp->arc_x_idx].ratio); interp->centre_point.vector[interp->arc_y_idx] = pi_round(interp->centre_point.vector[interp->arc_y_idx], interp->axis[interp->arc_y_idx].ratio); interp->axis_length.vector[interp->arc_x_idx] = (int)pi_round(radius_x, interp->axis[interp->arc_x_idx].ratio); interp->axis_length.vector[interp->arc_y_idx] = (int)pi_round(radius_y, interp->axis[interp->arc_y_idx].ratio); //计算8分圆的参考位置（相对圩圆心的绝对偏移） interp_calc_arc_refrence(interp); interp_task_init(interp, speed, stop_speed); } /** * 开始直线插补任务， * * @author LXZ (032620) * * @param interp 插补对象 * @param start_point 开始坐标 * @param end_point 终点坐标 * @param speed 速度 */ void interp_begin_line_task( interp_task_t *interp, axis_vector_t *start_point, axis_vector_t *end_point, int speed, int stop_speed ) { int number = 0; int i = 0; int max_axis_id = 0; int max_delta = 0; //如果轴无效，就不进入直线插补模式 //interp->axis[0].total_delta = abs(end_point->X - start_point->X); //interp->axis[1].total_delta = abs(end_point->Y - start_point->Y); //interp->axis[2].total_delta = abs(end_point->Z - start_point->Z); for (i = 0; i < INTERP_AXIS_NUMBER; i++) { interp->axis[i].total_delta = (int)(abs(end_point->vector[i] - start_point->vector[i]) * interp->axis[i].ratio); if (interp->axis[i].total_delta > 0) { interp_axis_enable(interp, i); } else { interp_axis_disable(interp, i); } if (interp->axis[i].flg != 0) { if (interp->axis[i].total_delta > max_delta) { max_delta = interp->axis[i].total_delta; max_axis_id = i; } number++; } } if (number < 1) { //单轴不需要插补 return; } interp->line_idx = max_axis_id; //两个轴以上时，最大进给轴就是插补轴 //interp->axis[max_axis_id].flg += 1; //有两个轴以上的插补，最大进给的轴就是主轴 interp->mode = INTERP_MODE_LINE; // interp->start_point = *start_point; interp->end_point = *end_point; //坐标比例变换 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { interp->start_point.vector[i] = pi_round(interp->start_point.vector[i], interp->axis[i].ratio); interp->end_point.vector[i] = pi_round(interp->end_point.vector[i], interp->axis[i].ratio); } //初始化轴参数 interp_task_init(interp, speed, stop_speed); } /** * 圆弧任务 * * @author LXZ (032720) * * @param interp 插补对象 * @param result 插补缓冲 */ static void interp_work_arc_task(interp_task_t *interp, interp_buffer_t *result) { interp_axis_t * axis1,*axis2; int cur_quadrant; int number = 0; int dir = 0; int count = result->count; int rel_period = interp->ref_clock; axis1 = &interp->axis[interp->arc_x_idx]; axis2 = &interp->axis[interp->arc_y_idx]; //进来的时候就需要进行一次减速位置初始化 if (axis1->dec_position == 0) { axis1->dec_speed = axis1->stop_speed; } if (axis2->dec_position == 0) { axis2->dec_speed = axis2->stop_speed; } while (count < INTERP_BUFFER_SIZE && (axis1->cur_delta > 0 || axis2->cur_delta > 0)) { //判断当前速度是否能进行加速 //求当前所在象限 //进行象限判断需要以轴长为标准 if (interp->axis_length.vector[axis1->id] >= interp->axis_length.vector[axis2->id]) { //以X轴为长轴进行判断 cur_quadrant = ellipse_get_eight_of_part_by_xaxis(interp->arc_dir, axis1->cur_posi - interp->centre_point.vector[axis1->id], axis2->cur_posi - interp->centre_point.vector[axis2->id], interp->quadrant_refrence.vector[axis1->id], interp->last_quadrant); } else { //以Y轴为长轴进行判断 cur_quadrant = ellipse_get_eight_of_part_by_yaxis(interp->arc_dir, axis1->cur_posi - interp->centre_point.vector[axis1->id], axis2->cur_posi - interp->centre_point.vector[axis2->id], interp->quadrant_refrence.vector[axis2->id], interp->last_quadrant); } //根据象限求出进给表 switch (cur_quadrant) { //根据8分圆来判断主轴的方向 case 1: case 4: case 5: case 8: //在这个区间里面，由于Y轴进给比Y轴大，因此，需要以Y轴为进给参考 if ( //cur_quadrant != interp->last_quadrant (interp->last_quadrant != 1) && (interp->last_quadrant != 4) && (interp->last_quadrant != 5) && (interp->last_quadrant != 8) ) { //象限变了 //主轴需要交替，因此需要根据上一次的速度值来重置判断条件 axis2->cur_speed = 0; //查找上一次的速度值 while (axis2->cur_speed + 1 < axis2->max_speed) { if (axis2->speed_table[axis2->cur_speed] >= axis2->last_feed) { break; } axis2->cur_speed++; } //初始化必须的减速脉冲 axis2->dec_speed = axis2->stop_speed; if (axis2->dec_speed <= axis2->cur_speed) { //当前速度比减速速高时，需要计算减速 axis2->dec_position = axis2->speed_table[axis2->dec_speed]; while (((axis2->dec_speed + 1) < axis2->max_speed) && (axis2->speed_table[axis2->dec_speed + 1] < axis2->speed_table[axis2->cur_speed])) { axis2->dec_speed++; axis2->dec_position += axis2->speed_table[axis2->dec_speed]; } } else { axis2->dec_position = 0; } } else if (axis2->cur_delta - axis2->dec_position > axis2->speed_table[axis2->cur_speed]) { //当前还允许进行该速度前进 //先判断减速 if ((axis2->dec_speed + 1 < axis2->max_speed) && axis2->speed_table[axis2->dec_speed + 1] < axis2->last_feed) { if (axis2->cur_delta - axis2->dec_position > axis2->speed_table[axis2->dec_speed + 1]) { //找出减速点 axis2->dec_speed++; axis2->dec_position += axis2->speed_table[axis2->dec_speed]; } } else { //计算Y轴下次进给值 number = axis2->speed_table[axis2->cur_speed]; axis2->last_period = rel_period / number; if (axis2->cur_delta < number) { number = axis2->cur_delta; } axis2->cur_delta -= number; //根据8分圆计算进给方向 if (interp->arc_dir == 0) { //逆时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { dir = 1; } else { dir = -1; } } else { //顺时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { dir = -1; } else { dir = 1; } } result->feed[count][axis2->id] = number * dir; axis2->cur_posi += result->feed[count][axis2->id]; result->period[count][axis2->id] = axis2->last_period; axis2->last_feed = number; //计算X轴在Y轴下一个位置下的进给坐标 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { number = (int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]), interp->axis_length.vector[axis2->id], interp->axis_length.vector[axis1->id]) + interp->centre_point.vector[axis1->id]; } else { number = -(int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]), interp->axis_length.vector[axis2->id], interp->axis_length.vector[axis1->id]) + interp->centre_point.vector[axis1->id]; } //判断进给方向 if (number == axis1->cur_posi) { if (interp->arc_dir == 0) { //逆时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = -1; } else { dir = 1; } } else { //顺时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = 1; } else { dir = -1; } } number = 0; } else if (number > axis1->cur_posi) { number = number - axis1->cur_posi; dir = 1; } else { number = axis1->cur_posi - number; dir = -1; } if (number != 0 && axis1->cur_delta > 0) { axis1->last_period = rel_period / number; result->period[count][axis1->id] = axis1->last_period; result->feed[count][axis1->id] = number * dir; axis1->cur_posi += result->feed[count][axis1->id]; axis1->last_feed = number; if (axis1->cur_delta > number) { axis1->cur_delta -= number; } else { axis1->cur_delta = 0; } } else { result->feed[count][axis1->id] = 0; axis1->last_feed = 0; axis1->last_period = PERIOD_MAX_VALUE; } if ((axis2->cur_delta - axis2->dec_position > axis2->speed_table[axis2->cur_speed]) && (axis2->cur_speed + 1) < axis2->max_speed) { //满足加速条件就进行加速 axis2->cur_speed++; } INTERP_FEED_DEBUG(++interp->feed_count); count++; } } else { //开始执行减速运动 if (axis2->cur_delta - (axis2->dec_position - axis2->speed_table[axis2->dec_speed]) >= axis2->speed_table[axis2->dec_speed]) { //如果剩余的脉冲是允许进行减速的，就继续在当前速度下进行运动来达到更快到达目的进给 //Y轴 number = axis2->speed_table[axis2->dec_speed]; axis2->last_period = rel_period / number; if (axis2->cur_delta < number) { number = axis2->cur_delta; } axis2->cur_delta -= number; if (interp->arc_dir == 0) { if (cur_quadrant == 3 || cur_quadrant == 4 || cur_quadrant == 5 || cur_quadrant == 6) { dir = -1; } else { dir = 1; } } else { if (cur_quadrant == 3 || cur_quadrant == 4 || cur_quadrant == 5 || cur_quadrant == 6) { dir = 1; } else { dir = -1; } } result->feed[count][axis2->id] = number * dir; axis2->cur_posi += result->feed[count][axis2->id]; result->period[count][axis2->id] = axis2->last_period; axis2->last_feed = number; //计算X轴在Y轴下一个位置下的进给坐标 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { number = (int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]), interp->axis_length.vector[axis2->id], interp->axis_length.vector[axis1->id]) + interp->centre_point.vector[axis1->id]; } else { number = -(int)eclipse_sqrt((axis2->cur_posi - interp->centre_point.vector[axis2->id]), interp->axis_length.vector[axis2->id], interp->axis_length.vector[axis1->id]) + interp->centre_point.vector[axis1->id]; } //判断进给方向 if (number == axis1->cur_posi) { if (interp->arc_dir == 0) { //逆时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = -1; } else { dir = 1; } } else { //顺时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = 1; } else { dir = -1; } } number = 0; } else if (number > axis1->cur_posi) { number = number - axis1->cur_posi; dir = 1; } else { number = axis1->cur_posi - number; dir = -1; } if (number != 0 && axis1->cur_delta > 0) { axis1->last_period = rel_period / number; result->period[count][axis1->id] = axis1->last_period; result->feed[count][axis1->id] = number * dir; axis1->cur_posi += result->feed[count][axis1->id]; axis1->last_feed = number; if (axis1->cur_delta > number) { axis1->cur_delta -= number; } else { axis1->cur_delta = 0; } } else { result->feed[count][axis1->id] = 0; axis1->last_feed = 0; axis1->last_period = PERIOD_MAX_VALUE; } INTERP_FEED_DEBUG(++interp->feed_count); count++; } else if (axis2->dec_speed > axis2->stop_speed) { axis2->dec_position -= axis2->speed_table[axis2->dec_speed]; axis2->dec_speed--; } else { axis2->dec_position = 0; } } break; case 2: case 3: case 6: case 7: //在这个区间里面，由于X轴进给比Y轴大，因此，需要以X轴为进给参考 if ( //cur_quadrant != interp->last_quadrant (interp->last_quadrant != 2) && (interp->last_quadrant != 3) && (interp->last_quadrant != 6) && (interp->last_quadrant != 7) ) { //象限变了之后，主轴需要交替，因此需要根据上一次的速度值来重置判断条件 //interp->last_quadrant = cur_quadrant; axis1->cur_speed = 0; //查找上一次的速度值 while ((axis1->cur_speed + 1) < axis1->max_speed) { if (axis1->speed_table[axis1->cur_speed] >= axis1->last_feed) { break; } axis1->cur_speed++; } //初始化必须的减速脉冲 axis1->dec_speed = axis1->stop_speed; if (axis1->cur_speed >= axis1->dec_speed) { //当当前速度比减速速高时，需要计算减速 axis1->dec_position = axis1->speed_table[axis1->dec_speed]; while (axis1->dec_speed + 1 < axis1->max_speed && axis1->speed_table[axis1->dec_speed + 1] < axis1->speed_table[axis1->cur_speed]) { axis1->dec_speed++; axis1->dec_position += axis1->speed_table[axis1->dec_speed]; } } else { axis1->dec_position = 0; } } else if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->cur_speed]) { if ((axis1->dec_speed + 1 < axis1->max_speed) && axis1->speed_table[axis1->dec_speed + 1] < axis1->last_feed) { if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->dec_speed + 1]) { axis1->dec_speed++; axis1->dec_position += axis1->speed_table[axis1->dec_speed]; } } else { //计算X轴下次进给值 number = axis1->speed_table[axis1->cur_speed]; axis1->last_period = rel_period / number; if (axis1->cur_delta < number) { number = axis1->cur_delta; } axis1->cur_delta -= number; //根据8分圆计算进给方向 if (interp->arc_dir == 0) { //逆时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = -1; } else { dir = 1; } } else { //顺时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = 1; } else { dir = -1; } } result->feed[count][axis1->id] = number * dir; axis1->cur_posi += result->feed[count][axis1->id]; result->period[count][axis1->id] = axis1->last_period; axis1->last_feed = number; //计算Y轴在X轴下一个位置下的进给坐标 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { number = (int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]), interp->axis_length.vector[axis1->id], interp->axis_length.vector[axis2->id]) + interp->centre_point.vector[axis2->id]; } else { number = -(int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]), interp->axis_length.vector[axis1->id], interp->axis_length.vector[axis2->id]) + interp->centre_point.vector[axis2->id]; } //判断进给方向 if (number == axis2->cur_posi) { number = 0; if (interp->arc_dir == 0) { //逆时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { dir = 1; } else { dir = -1; } } else { //顺时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { dir = -1; } else { dir = 1; } } } else if (number > axis2->cur_posi) { number = number - axis2->cur_posi; dir = 1; } else { number = axis2->cur_posi - number; dir = -1; } //计算轴2进给量 if (number > 0 && axis2->cur_delta > 0) { axis2->last_period = rel_period / number; result->period[count][axis2->id] = axis2->last_period; result->feed[count][axis2->id] = number * dir; axis2->cur_posi += result->feed[count][axis2->id]; axis2->last_feed = number; if (axis2->cur_delta > number) { axis2->cur_delta -= number; } else { axis2->cur_delta = 0; } } else { result->feed[count][axis2->id] = 0; axis2->last_feed = 0; axis2->last_period = PERIOD_MAX_VALUE; } if ((axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->cur_speed]) && (axis1->cur_speed + 1) < axis1->max_speed) { axis1->cur_speed++; } INTERP_FEED_DEBUG(++interp->feed_count); count++; } } else { //开始执行减速运动 if (axis1->cur_delta - (axis1->dec_position - axis1->speed_table[axis1->dec_speed]) >= axis1->speed_table[axis1->dec_speed]) { //如果剩余的脉冲是允许进行减速的，就继续在当前速度下进行运动来达到更快到达目的进给 //计算X轴下次进给值 number = axis1->speed_table[axis1->dec_speed]; axis1->last_period = rel_period / number; if (axis1->cur_delta < number) { number = axis1->cur_delta; } axis1->cur_delta -= number; //根据8分圆计算进给方向 if (interp->arc_dir == 0) { //逆时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = -1; } else { dir = 1; } } else { //顺时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { dir = 1; } else { dir = -1; } } result->feed[count][axis1->id] = number * dir; axis1->cur_posi += result->feed[count][axis1->id]; result->period[count][axis1->id] = axis1->last_period; axis1->last_feed = number; //计算Y轴在X轴下一个位置下的进给坐标 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 3 || cur_quadrant == 4) { number = (int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]), interp->axis_length.vector[axis1->id], interp->axis_length.vector[axis2->id]) + interp->centre_point.vector[axis2->id]; } else { number = -(int)eclipse_sqrt((axis1->cur_posi - interp->centre_point.vector[axis1->id]), interp->axis_length.vector[axis1->id], interp->axis_length.vector[axis2->id]) + interp->centre_point.vector[axis2->id]; } //判断进给方向 if (number == axis2->cur_posi) { number = 0; if (interp->arc_dir == 0) { //逆时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { dir = 1; } else { dir = -1; } } else { //顺时针 if (cur_quadrant == 1 || cur_quadrant == 2 || cur_quadrant == 7 || cur_quadrant == 8) { dir = -1; } else { dir = 1; } } } else if (number > axis2->cur_posi) { number = number - axis2->cur_posi; dir = 1; } else { number = axis2->cur_posi - number; dir = -1; } //计算轴2进给量 if (number > 0 && axis2->cur_delta > 0) { axis2->last_period = rel_period / number; result->period[count][axis2->id] = axis2->last_period; result->feed[count][axis2->id] = number * dir; axis2->cur_posi += result->feed[count][axis2->id]; axis2->last_feed = number; if (axis2->cur_delta > number) { axis2->cur_delta -= number; } else { axis2->cur_delta = 0; } } else { result->feed[count][axis2->id] = 0; axis2->last_feed = 0; axis2->last_period = PERIOD_MAX_VALUE; } INTERP_FEED_DEBUG(++interp->feed_count); count++; } else if (axis1->dec_speed > axis1->stop_speed) { axis1->dec_position -= axis1->speed_table[axis1->dec_speed]; axis1->dec_speed--; } else { axis1->dec_position = 0; } } break; } interp->last_quadrant = cur_quadrant; if (count > 0) { //当过程中双方进给都为0，说明运算其实结束了，只是因为计算差异会导致其中一个轴存在无法计算的余数 if (result->feed[count - 1][axis2->id] == 0 && result->feed[count - 1][axis1->id] == 0) { count--; axis1->cur_delta = 0; axis2->cur_delta = 0; break; } } } if (axis1->cur_delta == 0 && axis2->cur_delta == 0) { //插补运算完成 int speed = 0; int i = 0; interp->mode = INTERP_MODE_NONE; //需要计算一次完成后的每个轴当前速度 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { axis2 = &interp->axis[i]; speed = 0; while (axis2->speed_table[speed] < axis2->last_feed) { speed++; } axis2->cur_speed = speed; } } //result->used = 0; result->count = count; } /** * 直线插补 * * @author LXZ (032720) * * @param interp 插补对象 */ static void interp_work_line_task(interp_task_t *interp, interp_buffer_t *result) { interp_axis_t * axis1,*axis2; int number = 0; float ratio = 0; int count = result->count; int i = 0; int speed = 0; int rel_period = interp->ref_clock; axis1 = &interp->axis[interp->line_idx]; if (axis1->dec_position == 0) { axis1->dec_speed = axis1->stop_speed; if (axis1->cur_speed >= axis1->dec_speed) { axis1->dec_position = axis1->speed_table[axis1->dec_speed]; } } while (count < INTERP_BUFFER_SIZE && (axis1->cur_delta > 0)) { //判断当前速度是否能进行加速 if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->cur_speed]) { //计算减速距离 if (((axis1->dec_speed + 1) < axis1->max_speed) && (axis1->speed_table[axis1->dec_speed + 1] < axis1->last_feed)) { //先计算减速脉冲 if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->dec_speed + 1]) { //如果允许增加减速 axis1->dec_speed++; axis1->dec_position += axis1->speed_table[axis1->dec_speed]; } } else { //获取加速度寄存器值 number = axis1->speed_table[axis1->cur_speed]; axis1->last_period = rel_period / number; axis1->cur_delta -= number; axis1->last_feed = number; if (axis1->start_posi > axis1->target_posi) { axis1->cur_posi -= number; result->feed[count][axis1->id] = -number; //计算总进度比例 ratio = (float)(axis1->start_posi - axis1->cur_posi) / axis1->total_delta; } else { axis1->cur_posi += number; result->feed[count][axis1->id] = number; //计算总进度比例 ratio = (float)(axis1->cur_posi - axis1->start_posi) / axis1->total_delta; } result->period[count][axis1->id] = axis1->last_period; //计算总进度比例 //计算次轴的跟随进给 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { if (i != interp->line_idx) { //非主轴对象 axis2 = &interp->axis[i]; number = 0; if (axis2->flg != 0) { //选中为插补轴了 if (axis2->total_delta == axis1->total_delta) { //1比1时不需要计算 number = axis1->speed_table[axis1->cur_speed]; } else { if (axis2->start_posi > axis2->target_posi) { number = (int)(axis2->total_delta * ratio) - (axis2->start_posi - axis2->cur_posi); } else { number = (int)(axis2->total_delta * ratio) - (axis2->cur_posi - axis2->start_posi); } } } if (number > 0) { axis2->last_period = rel_period / number; if (axis2->start_posi > axis2->target_posi) { axis2->cur_posi -= number; result->feed[count][axis2->id] = -number; } else { axis2->cur_posi += number; result->feed[count][axis2->id] = number; } axis2->cur_delta -= number; result->period[count][axis2->id] = axis2->last_period; axis2->last_feed = number; } else { axis2->last_period = PERIOD_MAX_VALUE; result->feed[count][axis2->id] = 0; axis2->last_feed = 0; result->period[count][axis2->id] = PERIOD_MAX_VALUE; } } } //如果有条件加速就进行加速 if (axis1->cur_delta - axis1->dec_position > axis1->speed_table[axis1->cur_speed] && (axis1->cur_speed + 1) < axis1->max_speed) axis1->cur_speed++; INTERP_FEED_DEBUG(++interp->feed_count); count++; } } else { //开始执行减速运动 if (axis1->cur_delta - (axis1->dec_position - axis1->speed_table[axis1->dec_speed]) >= axis1->speed_table[axis1->dec_speed]) { number = axis1->speed_table[axis1->dec_speed]; axis1->last_period = rel_period / number; axis1->cur_delta -= number; axis1->last_feed = number; if (axis1->start_posi > axis1->target_posi) { axis1->cur_posi -= number; result->feed[count][axis1->id] = -number; //计算总进度比例 ratio = (float)(axis1->start_posi - axis1->cur_posi) / axis1->total_delta; } else { axis1->cur_posi += number; result->feed[count][axis1->id] = number; //计算总进度比例 ratio = (float)(axis1->cur_posi - axis1->start_posi) / axis1->total_delta; } result->period[count][axis1->id] = axis1->last_period; //计算次轴的跟随进给 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { if (i != interp->line_idx) { //非主轴对象 axis2 = &interp->axis[i]; number = 0; if (axis2->flg != 0) { //选中为插补轴了 if (axis2->total_delta == axis1->total_delta) { //1比1时不需要计算 number = axis1->speed_table[axis1->dec_speed]; } else { if (axis2->start_posi > axis2->target_posi) { number = (int)(axis2->total_delta * ratio) - (axis2->start_posi - axis2->cur_posi); } else { number = (int)(axis2->total_delta * ratio) - (axis2->cur_posi - axis2->start_posi); } } } if (number > 0) { axis2->last_period = rel_period / number; if (axis2->start_posi > axis2->target_posi) { axis2->cur_posi -= number; result->feed[count][axis2->id] = -number; } else { axis2->cur_posi += number; result->feed[count][axis2->id] = number; } axis2->cur_delta -= number; result->period[count][axis2->id] = axis2->last_period; axis2->last_feed = number; } else { axis2->last_period = PERIOD_MAX_VALUE; result->feed[count][axis2->id] = 0; axis2->last_feed = 0; result->period[count][axis2->id] = PERIOD_MAX_VALUE; } } } INTERP_FEED_DEBUG(++interp->feed_count); count++; } else if (axis1->dec_speed > 0) { axis1->dec_position -= axis1->speed_table[axis1->dec_speed]; axis1->dec_speed--; } else { axis1->dec_position = 0; } } } if (axis1->cur_delta == 0) { interp->mode = INTERP_MODE_NONE; axis1->cur_speed = axis1->dec_speed; //需要计算一次完成后的每个轴当前速度 for (i = 0; i < INTERP_AXIS_NUMBER; i++) { if (i != interp->line_idx) { //非主轴对象 axis2 = &interp->axis[i]; speed = 0; while (axis2->speed_table[speed] < axis2->last_feed) { speed++; } axis2->cur_speed = speed; } } } result->count = count; //result->used = 0; } /** * 插补减速停止 * * @author LXZ (033120) * * @param interp 插补对象 */ void interp_task_stop(interp_task_t *interp) { interp->stop_flg = 1; //打开了停止位，周期进给会停止 } /** * 执行插补任务 * * @author LXZ (032620) * * @param interp 插补对象 */ void interp_task_run(interp_task_t *interp) { interp_buffer_t *buffer = (void *)0; int i = 0; if (!interp_is_fin(interp) && interp->stop_flg == 0) { //有任务并且没有打开停止位时，开始进给 //查找一个空的可用缓冲 for (i = 0; i < INTERP_WORK_BUFFER_NUMBER; i++) { if (interp->work_buffer[i].count <= interp->work_buffer[i].used) { buffer = &interp->work_buffer[i]; memset(buffer, 0, sizeof(interp_buffer_t)); interp->work_buffer[i].used = INTERP_BUFFER_SIZE + 1; interp->work_buffer[i].count = 0; break; } } if (buffer != (void *)0) { //取得有效插补缓冲 do { if (interp->mode == INTERP_MODE_ARC) { //圆弧插补 interp_work_arc_task(interp, buffer); } else if (interp->mode == INTERP_MODE_LINE) { //直线插补 interp_work_line_task(interp, buffer); } else if (interp->path_count > interp->cur_path_index) { //查找下一个路径 interp_path_t *path = &interp->paths[interp->cur_path_index]; int i = 0; axis_vector_t sp = { 0 }; axis_vector_t ep = { 0 }; axis_vector_t cp = { 0 }; #if 0 if (interp->cur_path_index > 0) { interp_path_t *path0 = &interp->paths[interp->cur_path_index - 1]; for (i = 0; i < INTERP_AXIS_NUMBER; i++) { //修正目标位置，由于上次运算在计算圆弧的时候会引入误差，在运算完后需要修正 ep.vector[i] = path->end_point.vector[i] - path0->end_point.vector[i] + interp->axis[i].cur_posi - interp->axis[i].target_posi; cp.vector[i] = path->centre_point.vector[i] - path0->end_point.vector[i] + interp->axis[i].cur_posi - interp->axis[i].target_posi; } } else { for (i = 0; i < INTERP_AXIS_NUMBER; i++) { ep.vector[i] = path->end_point.vector[i]; cp.vector[i] = path->centre_point.vector[i]; } } #else for (i = 0; i < INTERP_AXIS_NUMBER; i++) { ep.vector[i] = path->end_point.vector[i]; cp.vector[i] = path->centre_point.vector[i]; } #endif if (path->work_type == INTERP_MODE_LINE) { //直线插补 //开始直线任务 interp_begin_line_task(interp, &sp, &ep, path->target_speed, path->stop_speed); } else if (path->work_type == INTERP_MODE_ARC) { //圆弧插补 //设置选用轴 interp_set_arc_axis(interp, path->arc_x_id, path->arc_y_id); //开始圆弧任务 interp_begin_arc_task(interp, &sp, &ep, &cp, path->wor_dir, path->target_speed, path->stop_speed); } else if (path->work_type == INTERP_MODE_ELLIPSE) { //设置选用轴 interp_set_arc_axis(interp, path->arc_x_id, path->arc_y_id); //开始椭圆弧任务 interp_begin_ellipse_task(interp, &sp, &ep, &cp, path->radius_x, path->radius_y, path->wor_dir, path->target_speed, path->stop_speed); } interp->cur_path_index++; } else { break; } } while (buffer->count < INTERP_BUFFER_SIZE); if (buffer->count > 0) { buffer->used = 0; //使能该缓冲输出 } } } } /** * 插补周期运行函数，需要定时调用 * * @author LXZ (032620) * * @param interp 插补对象 */ void interp_task_period(interp_task_t *interp) { int i = 0; if (interp->stop_flg == 0) { interp_buffer_t *buffer = &interp->work_buffer[interp->cur_buffer_index]; if (buffer->used < buffer->count) { for (i = 0; i < INTERP_AXIS_NUMBER; i++) { if (buffer->feed[buffer->used][i] != 0) { //插补数据有效时输出到物理轴 interp->axis[i].real_posi += buffer->feed[buffer->used][i]; //计算新的真实单位位置 interp->axis[i].cur_feed = abs(buffer->feed[buffer->used][i]); interp_axis_period(&interp->axis[i], buffer->feed[buffer->used][i], INTERP_REAL_PERIOD(buffer->period[buffer->used][i])); } else { interp->axis[i].cur_feed = 0; } } INTERP_PERIOD_REPORT(buffer); buffer->used++; if (buffer->used >= buffer->count) { //插补缓冲完成了 buffer->count = 0; buffer->used = 0; if (++interp->cur_buffer_index >= INTERP_WORK_BUFFER_NUMBER) { //切换缓冲 interp->cur_buffer_index = 0; } } } } } /** * 返回插补动作是否完成 * * @author LXZ (032620) * * @param interp 插补对象 * * @return int 返回0 表示未完成，返回1表示完成 */ int interp_is_fin(interp_task_t *interp) { int i = 0; if (interp->path_count > 0 && interp->cur_path_index < interp->path_count) //路径缓冲池没完 return 0; for (i = 0; i < INTERP_AXIS_NUMBER; i++) { //轴还在运行 if (interp_axis_is_runnging(&interp->axis[i])) { return 0; } } for (i = 0; i < INTERP_DEC_BUFFER_NUMBER; i++) { //插补缓冲还没执行完 if (interp->work_buffer[i].count > interp->work_buffer[i].used) { return 0; } } //当前运算任务也已经完成 return interp->mode == INTERP_MODE_NONE; }