GC24001/Compenent/axis_motion/axis_interp.c

//============================================================================//
//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 <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#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;
}