GC24001/Compenent/axis_motion/axis_motion.c

//===================================修改记录=================================//
//2020.05.12 11:48 LXZ
//	1、取消speed_unit参数，所以参数外部自己需要知道倍率，内部只管真实频率
//	2、增加了设置正反转与高速回零的参数设置
//	3、回零增加信号管脚、滤波次数的设置项目
//  4、增加了回零方式中断回零的实现
//2020.05.14 00:18 LXZ
//	1、增了设置软件限位点的接口，修改软件限位开启接口
//	2、增加了若干对外的参数读写类型
//	3、修改报警反馈方式为位标志
//2020.05.18 15:14 LXZ
//  1、增加设置编码器回零的接口
//  2、去除对编码器管脚的参数设置，要求必须用接口去操作
//  3、增加对编码器Z信号回零的功能代码
//  4、增加了设置齿轮比的对应接口
//  5、增加了设置与读取现实位置的参数对象
//  6、增加了位置与脉冲单位转换接口
//  7、增加了转速与频率单位转换接口
//  8、对位置定位的相关的代码自动调用位置与脉冲单位转换
//  9、增加方向读取对象
//2020.05.21 18:05 LXZ
//  1、修正加减速状态值的定义
//  2、修正位置减停位置不够时变速标志位设置不正常的问题
//  3、修正回零方式1的回零偏移量使用错误的问题
//  4、修改多段速函数接口定义
//  5、修正软件限位设置接口没有齿轮比转换的问题
//2020.05.26 14:49
//  1、axis_pp函数与axis_start_pp_task函数增加返回值来表示定向运动是否正常启动
//  2、增加报警
//  3、增加axis_clear_alarm来清除部分非实时报警
//  4、统一一下报警接口名称
//2020.06.10 15:49
//  1、增加关闭限位使能时自动清除相关报警位
//  2、PP定位过程增加防止尖顶的过程的限制
//2020.06.13 11:21
//  1、增加了加减速曲线2与加减速曲线3，需要手动设置加加速与最大加减速，加减时间无效化
//     加减速曲线2与加减速曲线1曲线形状保持一致，但是依靠手动设置加加速，因此不需要设
//     置加减速时间去匹配每个阶段的速度
//     加减速曲线3同理，但是曲线3只有波形下半部分是S形，上半部分是直线，因此加速时间比
//     加减速曲线2要少，结速部分确仍然能保证一定的平滑
//2020.06.17 11:21
//  1、在插补进给中添加死等代码来保证外围运算波动不会对其造成影响
//  2、将预览版本的比例开关并入驱动，增加比例开关接口，
//     当比例开关打开时，所有的位置相关移动与限位设置都会经过比例计算
//     当开关关闭时，所有定位相关的接口与限位设置都以脉冲为单位，但是位置与脉冲的转换接口是可用的。
//  3、修正软件限位报警自动清零功能。
//  4、增加减速设置项目
//2020.06.20 00:54
//  1、修改齿轮比接口，增加一个放大比例参数 
//  2、定位接口平滑倍率改成宏定义，并加大
//  3、修改了脉冲、位置转换计算方法，采用预比例计算方式，提升运算速度
//2020.06.28 00:54
//  1、根据侧孔钻应用定向修改了齿轮比设置接口，同时完全照搬转换代码
//  2、全并侧孔钻增加速度转频率接口
//  3、为了剥离硬件IO接口，修改原点获取硬件信号方式为指针地址加掩码的方式
//  4、对应修改回零管脚的注册管脚接口
//============================================================================//

#include "axis_motion.h"
#include "axis_motion_config.h"
#include <string.h>
#include "user_app.h"


/**
 * 轴运动驱动是一个利用时间为单位进行步进加减速度的驱动库。由于采用了定时周期加减速度，因此可以避免步进电机与 
 * 伺服电机因为脉冲周期问题导致加减速度不好控制以及不好确定加减脉冲的问题。另外可以通过引入多段加减速度来保证 
 * 停止的时候与开始时候的顺滑度，保证不会因为中间段加减速度过大而过冲。
 *  
 */




/**
 * 轴使能
 * 
 * @author LXZ (121919)
 * 
 * @param axis 
 */
void axis_enable(axis_object_t *axis) {
    axis->outputs.enable = 1;
    AXIS_SET_EN(axis, 1);
}

/**
 * 轴禁止
 * 
 * @author LXZ (121919)
 * 
 * @param axis 
 */
void axis_disable(axis_object_t *axis) {
    axis->outputs.enable = 0;
    AXIS_SET_EN(axis, 0);
}

/**
 * 轴正转
 * 
 * @author lxz 
 * 
 * @param axis 
 */
void axis_cw(axis_object_t *axis) {
    axis->outputs.dir = 1;
    axis->feed = 1;
    if (axis->inputs.dir_reverse) {
        AXIS_SET_DIR(axis, 0);
    }
    else {
        AXIS_SET_DIR(axis, 1);
    }
}
/**
 * 轴反转
 * 
 * @author lxz 
 * 
 * @param axis 
 */
void axis_ccw(axis_object_t *axis) {
    axis->outputs.dir = 0;
        axis->feed = -1;
    if (axis->inputs.dir_reverse) {
        AXIS_SET_DIR(axis, 1);
    }
    else {
        AXIS_SET_DIR(axis, 0);
    }
}


#ifdef AXIS_USING_FLOAT

/**
 * 以直线加减速的方式计算加减速表
 * 
 * @author LXZ (021720)
 * 
 * @param axis 
 * @param target_speed 
 */
static void axis_mode0_calc_accdec(axis_object_t *axis, int target_speed) {
    int acc_steps = 0;      //加速时间单位数
    int dec_steps = 0;      //减速时间单位数
    float cur_max_accdec = 0; //当前最大加速度减速度
    float cur_speed = 0;   //当前速度
    int index = 0;
    int clock = axis->clock;
    int start_speed = axis->start_speed;
    float clock_ratio = axis->clock / 1000000;
    //周期临时值
    int period;
    int reg_Value = 0;
    int last_reg_value = 0;

    period = axis->period;
    //限制加速度周期
    if (period < 1) period = 2;

    //限制最小加速度时间单位
    acc_steps = axis->acc_time / period;
    if (acc_steps < 10) {
        acc_steps = 10;
    }

    dec_steps = axis->dec_time / period;
    if (dec_steps < 10) {
        dec_steps = 10;
    }
    //转换倍率
    period *= 1000;

    //保证加减速尽量对齐，需要保证加减数是偶数，由于计算误差，加1能让结果更加逼近设置的时间
    acc_steps++;
    acc_steps >>= 1;
    dec_steps++;
    dec_steps >>= 1;

    //保证是偶次数加减数，能减少计算误差
    //if ((min_acc_step & 0x01) != 0)
    //    min_acc_step++;
    //目标速度不能比开始速度小
    if (target_speed < start_speed) {
        target_speed = start_speed;
    }

    //计算加速度会达到的最大值
    cur_max_accdec = (float)(target_speed - axis->start_speed) / acc_steps;
    //保证至少1以上的加减速度
    if (cur_max_accdec < 1) {
        cur_max_accdec = 1;
    }
    //尽量保证最大加速度不会大于限制值
    if (cur_max_accdec > axis->max_acc) {
        cur_max_accdec = axis->max_acc;
    }

    cur_speed = start_speed;
    //S加速曲线加速表计算
    do {
        //计算速度
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_ACC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_ACC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value);
            if (AXIS_ACC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_ACC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->acc_speed_table[index] = (int)cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        cur_speed += cur_max_accdec;

    } while ((cur_speed < target_speed) && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存表大小
    axis->acc_table_size = index;
#ifdef AXIS_USING_ACC_TABLE_ONLY
    if (index > 0) {
        index -= 1;
    }
    axis->dec_table_size = index;
#else

    cur_max_accdec = (target_speed - axis->start_speed) / dec_steps;
    //保证至少1以上的加减速度
    if (cur_max_accdec < 1) {
        cur_max_accdec = 1;
    }
    //计算加速度会达到的最大值
    if (cur_max_accdec > axis->max_acc) {
        cur_max_accdec = axis->max_acc;
    }

    dec_steps = (int)((target_speed - axis->start_speed) / cur_max_accdec);

    //开始减速表计算
    index = 0;
    last_reg_value = 0;

    cur_speed = start_speed;
    //S加速曲线减速表计算
    do {
        //计算速度
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_DEC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_DEC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio  / reg_Value);
            if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
#ifdef AXIS_USING_DEBUG
            axis->dec_speed_table[index] = (int)cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        cur_speed += cur_max_accdec;

    } while ((cur_speed < target_speed) && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存减速表大小
    axis->dec_table_size = index;
    if (axis->dec_table_size > 0) {
        axis->dec_table_size -= 1;
    }
#endif
}

/**
 * 以S曲线减速的方式计算加减速表
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象
 * @param target_speed 目标速度
 */
static void axis_mode1_calc_accdec(axis_object_t *axis, int target_speed) {
    int acc_steps = 0;      //加速时间单位数
    int dec_steps = 0;      //减速时间单位数
    float cur_accdec = 0;  //当前加减速度
    float cur_max_accdec = 0; //当前最大加速度减速度
    float cur_speed = 0;   //当前速度
    float ddc_speed = 0;   //减减速开始速度
    float cur_aacddc = 0;  //当前加加速减减速
    float mid_speed = 0;   //中间速度
    int index = 0;
    float clock = axis->clock;
    float clock_ratio = axis->clock / 1000000;
    int start_speed = axis->start_speed;
    //周期临时值
    int period;
    int reg_Value = 0;
    int last_reg_value = 0;

    period = axis->period;
    //限制加速度周期
    if (period < 1) period = 2;

    //限制最小加速度时间单位
    acc_steps = axis->acc_time / period;
    if (acc_steps < 10) {
        acc_steps = 10;
    }

    dec_steps = axis->dec_time / period;
    if (dec_steps < 10) {
        dec_steps = 10;
    }
    //转换倍率
    period *= 1000;

    //保证加减速尽量对齐，需要保证加减数是偶数，由于计算误差，加1能让结果更加逼近设置的时间
    acc_steps++;
    acc_steps >>= 1;
    dec_steps++;
    dec_steps >>= 1;

    //保证是偶次数加减数，能减少计算误差
    //if ((min_acc_step & 0x01) != 0)
    //    min_acc_step++;
    //目标速度不能比开始速度小
    if (target_speed < start_speed) {
        target_speed = start_speed;
    }

    //S加速算法需要先计算出加加速
    //推导过程：
    //S加减速度分为加加速与减加速度两个过程，假定两个过程是对称的，那么总占用时间为加速时间t的一半
    //因此利用积分公式(dv = sadt, vm = (vd - v0) / 2 => a = vm / (2 * (t / 2))可以得到acc = (vd - v0) * 2 / t
    //其中a与acc表示加速度，vd表示目标速度，v0表示开始速度t表示加速时间一半
    //加加速度aac与加速度关系acc为acc = aac * (t / 2)
    //从而公式为aac = (vd - v0) * 4 / (t ^2)
    //其中acc表示加速度，aac表示加加速，vd为目标速度，v0为开始速度，t为总的加速时间，在这算法里它就是加速度步数
    //由于加速度步数要对称就必须保持偶数，加上一些计算偏差以及为了保证电机正常运算必须限制参数
    //因此实际上两个过程在实际上是很难对称的，这就造成了实际加速时间肯定会比设定的长，具体长多
    //跟最大加加速，最大中速度，最小脉冲数，计算偏差，最小加速度步数有关
    cur_aacddc = ((target_speed - axis->start_speed)) / (acc_steps * acc_steps);

    //保证至少1以上的加减速度
    if (cur_aacddc < 1) {
        cur_aacddc = 1;
    }
    //计算加速度会达到的最大值
    cur_max_accdec = cur_aacddc * acc_steps;
    if (cur_max_accdec > axis->max_acc) {
        cur_max_accdec = axis->max_acc;
    }

    //限制加加速度，加加速度过大会导致过冲
    if (cur_aacddc > axis->max_aac) {
        cur_aacddc = axis->max_aac;
    }



    //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
    acc_steps = (int)(cur_max_accdec / cur_aacddc);

    if (cur_aacddc == 0) cur_aacddc = 1;



    //开始计算加速表
    mid_speed = (target_speed + axis->start_speed) / 2;
    ddc_speed = target_speed - ((cur_aacddc * acc_steps * acc_steps) / 2);
    if (ddc_speed < mid_speed) {
        ddc_speed = mid_speed;
    }
    cur_speed = axis->start_speed;


    //S加速曲线加速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长
        if (cur_speed >= mid_speed) {
            cur_accdec = cur_max_accdec;
            //保证只能进一次
            mid_speed *= 3;
        }

        if (cur_speed >= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (cur_accdec > cur_aacddc) {
                cur_accdec -= cur_aacddc;
            }
            else {
                //保证至少有这个加加速，让速度有机会达到目标速度
                cur_accdec = cur_aacddc;
            }
        }

        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_ACC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_ACC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value);
            if (AXIS_ACC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_ACC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->acc_speed_table[index] = (int)cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < ddc_speed) {        //还没到开始减速的速度
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }
        }

    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存表大小
    axis->acc_table_size = index;
#ifdef AXIS_USING_ACC_TABLE_ONLY
    if (index > 0) {
        index -= 1;
    }
    axis->dec_table_size = index;
#else
    if (dec_steps != acc_steps) {
        cur_aacddc = ((target_speed - axis->start_speed)) / (dec_steps * dec_steps);
        //保证至少1以上的加减速度
        if (cur_aacddc < 1) {
            cur_aacddc = 1;
        }
        //计算加速度会达到的最大值
        cur_max_accdec = cur_aacddc * dec_steps;
        if (cur_max_accdec > axis->max_acc) {
            cur_max_accdec = axis->max_acc;
        }

        //限制加加速度，加加速度过大会导致过冲
        if (cur_aacddc > axis->max_aac) {
            cur_aacddc = axis->max_aac;
        }
        //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
        dec_steps = (int)(cur_max_accdec / cur_aacddc);
        //curMaxAccDec = curAacDdc * decSteps;
        //if (cur_aacddc == 0) cur_aacddc = 1;
    }

    //如果减速时间一样，就不重复计算表了
    if (dec_steps == acc_steps) {
        int j = 0;
        while (j < index) {
            AXIS_DEC_REG_TABLE(axis)[j] = AXIS_ACC_REG_TABLE(axis)[j];
            AXIS_DEC_NUMBER_TABLE(axis)[j] = AXIS_ACC_NUMBER_TABLE(axis)[j];
            j++;
        }
        axis->dec_table_size = index;
        if (axis->dec_table_size > 0) {
            axis->dec_table_size -= 1;
        }
        return;
    }

    //开始减速表计算
    index = 0;
    last_reg_value = 0;
    mid_speed = (target_speed + axis->start_speed) / 2;
    ddc_speed = target_speed - ((cur_aacddc * dec_steps * dec_steps) / 2);

    if (ddc_speed < mid_speed) {
        ddc_speed = mid_speed;
    }

    cur_speed = axis->start_speed;

    //S加速曲线减速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长
        if (cur_speed >= mid_speed) {
            cur_accdec = cur_max_accdec;
            //保证只可能进一次
            mid_speed *= 3;
        }
        if (cur_speed >= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (cur_accdec > cur_aacddc) {
                cur_accdec -= cur_aacddc;
            }
            else {
                //保证至少有这个加加速，让速度有机会达到目标速度
                cur_accdec = cur_aacddc;
            }
        }

        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);

        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_DEC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_DEC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio  / reg_Value);
            if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->dec_speed_table[index] = (int)cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < ddc_speed) {        //还没到开始减速的速度
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }
        }

    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存减速表大小
    axis->dec_table_size = index;
    if (axis->dec_table_size > 0) {
        axis->dec_table_size -= 1;
    }
#endif
}



/**
 * 以定加加速的S曲线方式计算加减速表
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象
 * @param target_speed 目标速度
 */
static void axis_mode2_calc_accdec(axis_object_t *axis, int target_speed) {
    float cur_accdec = 0;  //当前加减速度
    float cur_max_accdec = 0; //当前最大加速度减速度
    float cur_speed = 0;   //当前速度
    float ddc_speed = 0;   //减减速开始速度
    float cur_aacddc = 0;  //当前加加速减减速
    float mid_speed = 0;   //中间速度
    int index = 0;
    float clock = axis->clock;
    float clock_ratio = axis->clock / 1000000;
    int start_speed = axis->start_speed;
    //周期临时值
    int period;
    int reg_Value = 0;
    int last_reg_value = 0;

    period = axis->period;
    //限制加速度周期
    if (period < 1) period = 1;

    //转换倍率
    period *= 1000;

    //目标速度不能比开始速度小
    if (target_speed < start_speed) {
        target_speed = start_speed;
    }





    //开始计算加速表
    mid_speed = (target_speed + axis->start_speed) / 2;
    cur_speed = axis->start_speed;

    cur_aacddc = axis->max_aac;
    cur_max_accdec = axis->max_acc;
    cur_accdec = 0;
    ddc_speed = 0;
    //S加速曲线加速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长

        if (cur_speed >= mid_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (ddc_speed != 0) {   //ddc_speed是为了保证第一次开始减速的时候,加加速不掉下来,从而保证曲线中心至少在上方
                if (cur_accdec > cur_aacddc) {
                    cur_accdec -= cur_aacddc;
                }
                else {
                    //保证至少有这个加加速，让速度有机会达到目标速度
                    cur_accdec = cur_aacddc;
                }
            }
            else {
                ddc_speed = mid_speed;
            }
        }
        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_ACC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_ACC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value);
            if (AXIS_ACC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_ACC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->acc_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < mid_speed) {
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }
        }
    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存表大小
    axis->acc_table_size = index;

#ifdef AXIS_USING_ACC_TABLE_ONLY
    if (index > 0) {
        index -= 1;
    }
    axis->dec_table_size = index;
#else


    //如果减速时间一样，就不重复计算表了
    if (axis->max_dec == axis->max_dec) {
        int j = 0;
        while (j < index) {
            AXIS_DEC_REG_TABLE(axis)[j] = AXIS_ACC_REG_TABLE(axis)[j];
            AXIS_DEC_NUMBER_TABLE(axis)[j] = AXIS_ACC_NUMBER_TABLE(axis)[j];
            j++;
        }
        axis->dec_table_size = index;
        if (axis->dec_table_size > 0) {
            axis->dec_table_size -= 1;
        }
        return;
    }

    //开始减速表计算
    index = 0;
    last_reg_value = 0;
    mid_speed = (target_speed + axis->start_speed) / 2;
    ddc_speed = 0;
    cur_accdec = 0;
    cur_max_accdec = axis->max_dec;
    cur_speed = axis->start_speed;

    //S加速曲线减速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长
        if (cur_speed >= mid_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (ddc_speed != 0) {
                if (cur_accdec > cur_aacddc) {
                    cur_accdec -= cur_aacddc;
                }
                else {
                    //保证至少有这个加加速，让速度有机会达到目标速度
                    cur_accdec = cur_aacddc;
                }
            }
            else {
                ddc_speed = mid_speed;
            }
        }

        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_DEC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_DEC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value);
            if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->dec_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < mid_speed) {        //还没到开始减速的速度
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }
        }

    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存减速表大小
    axis->dec_table_size = index;
    if (axis->dec_table_size > 0) {
        axis->dec_table_size -= 1;
    }
#endif
}


/**
 * 以定加加速的S曲线方式计算加减速表
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象
 * @param target_speed 目标速度
 */
static void axis_mode3_calc_accdec(axis_object_t *axis, int target_speed) {
    float cur_accdec = 0;  //当前加减速度
    float cur_max_accdec = 0; //当前最大加速度减速度
    float cur_speed = 0;   //当前速度
    //float ddc_speed = 0;   //减减速开始速度
    float cur_aacddc = 0;  //当前加加速减减速
    //float mid_speed = 0;   //中间速度
    int index = 0;
    float clock = axis->clock;
    float clock_ratio = axis->clock / 1000000;
    int start_speed = axis->start_speed;
    //周期临时值
    int period;
    int reg_Value = 0;
    int last_reg_value = 0;

    period = axis->period;
    //限制加速度周期
    if (period < 1) period = 1;

    //转换倍率
    period *= 1000;

    //目标速度不能比开始速度小
    if (target_speed < start_speed) {
        target_speed = start_speed;
    }





    //开始计算加速表
    //mid_speed = (target_speed + axis->start_speed) / 2;
    cur_speed = axis->start_speed;

    cur_aacddc = axis->max_aac;
    cur_max_accdec = axis->max_acc;
    cur_accdec = 0;
    //ddc_speed = 0;
    //S加速曲线加速表计算
    do {
        //变异的S曲线，没有上部
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_ACC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_ACC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value);
            if (AXIS_ACC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_ACC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->acc_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif

        //加加速流程
        cur_accdec += cur_aacddc;
        if (cur_accdec >= cur_max_accdec) {
            cur_accdec = cur_max_accdec;
        }

    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存表大小
    axis->acc_table_size = index;

#ifdef AXIS_USING_ACC_TABLE_ONLY
    if (index > 0) {
        index -= 1;
    }
    axis->dec_table_size = index;
#else


    //如果减速时间一样，就不重复计算表了
    if (axis->max_dec == axis->max_dec) {
        int j = 0;
        while (j < index) {
            AXIS_DEC_REG_TABLE(axis)[j] = AXIS_ACC_REG_TABLE(axis)[j];
            AXIS_DEC_NUMBER_TABLE(axis)[j] = AXIS_ACC_NUMBER_TABLE(axis)[j];
            j++;
        }
        axis->dec_table_size = index;
        if (axis->dec_table_size > 0) {
            axis->dec_table_size -= 1;
        }
        return;
    }

    //开始减速表计算
    index = 0;
    last_reg_value = 0;
    //mid_speed = (target_speed + axis->start_speed) / 2;
    //ddc_speed = 0;
    cur_accdec = 0;
    cur_max_accdec = axis->max_dec;
    cur_speed = axis->start_speed;

    //S加速曲线减速表计算
    do {
        //变异的S曲线，上升不减速，
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = (int)((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_DEC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_DEC_NUMBER_TABLE(axis)[index] = (int)((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value);
            if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->dec_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        //加加速流程
        cur_accdec += cur_aacddc;
        if (cur_accdec >= cur_max_accdec) {
            cur_accdec = cur_max_accdec;
        }
    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存减速表大小
    axis->dec_table_size = index;
    if (axis->dec_table_size > 0) {
        axis->dec_table_size -= 1;
    }
#endif
}

#else

/**
 * 以直线加减速的方式计算加减速表
 * 
 * @author LXZ (021720)
 * 
 * @param axis 
 * @param target_speed 
 */
static void axis_mode0_calc_accdec(axis_object_t *axis, int target_speed) {
    int acc_steps = 0;      //加速时间单位数
    int dec_steps = 0;      //减速时间单位数
    int cur_max_accdec = 0; //当前最大加速度减速度
    int cur_speed = 0;   //当前速度
    int index = 0;
    int step = 0;
    int clock = axis->clock;
    int clock_ratio = axis->clock / 1000000;
    int start_speed = axis->start_speed;
    //周期临时值
    int period;
    int reg_Value = 0;
    int last_reg_value = 0;

    period = axis->period;
    //限制加速度周期
    if (period < 1) period = 2;

    //限制最小加速度时间单位
    acc_steps = axis->acc_time / period;
    if (acc_steps < 10) {
        acc_steps = 10;
    }

    dec_steps = axis->dec_time / period;
    if (dec_steps < 10) {
        dec_steps = 10;
    }
    //转换倍率
    period *= 1000;

    //保证加减速尽量对齐，需要保证加减数是偶数，由于计算误差，加1能让结果更加逼近设置的时间
    acc_steps++;
    acc_steps >>= 1;
    dec_steps++;
    dec_steps >>= 1;

    //保证是偶次数加减数，能减少计算误差
    //if ((min_acc_step & 0x01) != 0)
    //    min_acc_step++;
    //目标速度不能比开始速度小
    if (target_speed < start_speed) {
        target_speed = start_speed;
    }

    //计算加速度会达到的最大值
    cur_max_accdec = (target_speed - axis->start_speed) / acc_steps;
    //保证至少1以上的加减速度
    if (cur_max_accdec < 1) {
        cur_max_accdec = 1;
    }
    //尽量保证最大加速度不会大于限制值
    if (cur_max_accdec > axis->max_acc) {
        cur_max_accdec = axis->max_acc;
    }
    acc_steps = (target_speed - axis->start_speed) / cur_max_accdec;
    //S加速曲线加速表计算
    do {
        //计算速度
        cur_speed = ((target_speed - start_speed) * step) / acc_steps + start_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = ((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_ACC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_ACC_NUMBER_TABLE(axis)[index] = ((period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value;
                                                  if (AXIS_ACC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_ACC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
                                                  axis->acc_speed_table[index] = cur_speed;
#endif
                                                  index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        step++;
    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存表大小
    axis->acc_table_size = index;

#ifdef AXIS_USING_ACC_TABLE_ONLY
    if (index > 0) {
        index -= 1;
    }
    axis->dec_table_size = index;
#else
    cur_max_accdec = (target_speed - axis->start_speed)/ dec_steps;
    //保证至少1以上的加减速度
    if (cur_max_accdec < 1) {
        cur_max_accdec = 1;
    }
    //计算加速度会达到的最大值
    if (cur_max_accdec > axis->max_acc) {
        cur_max_accdec = axis->max_acc;
    }
    dec_steps = (target_speed - axis->start_speed)/ cur_max_accdec;

    //开始减速表计算
    index = 0;
    last_reg_value = 0;
    step = 0;

    //S加速曲线减速表计算
    do {
        //计算速度
        cur_speed = (target_speed - start_speed) * step / dec_steps + start_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速

        reg_Value = ((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_DEC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_DEC_NUMBER_TABLE(axis)[index] = (period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value;
            if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
#ifdef AXIS_USING_DEBUG
            axis->dec_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        step++;
    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存减速表大小
    axis->dec_table_size = index;
    if (axis->dec_table_size > 0) {
        axis->dec_table_size -= 1;
    }
#endif
}

/**
 * 以S曲线减速的方式计算加减速表
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象
 * @param target_speed 目标速度
 */
static void axis_mode1_calc_accdec(axis_object_t *axis, int target_speed) {
    int acc_steps = 0;      //加速时间单位数
    int dec_steps = 0;      //减速时间单位数
    int cur_accdec = 0;  //当前加减速度
    int cur_max_accdec = 0; //当前最大加速度减速度
    int cur_speed = 0;   //当前速度
    int ddc_speed = 0;   //减减速开始速度
    int cur_aacddc = 0;  //当前加加速减减速
    int mid_speed = 0;   //中间速度
    int index = 0;
    int clock = axis->clock;
    int clock_ratio = axis->clock / 1000000;
    int start_speed = axis->start_speed;
    //周期临时值
    int period;
    int reg_Value = 0;
    int last_reg_value = 0;

    period = axis->period;
    //限制加速度周期
    if (period < 1) period = 1;

    //限制最小加速度时间单位
    acc_steps = axis->acc_time / period;
    if (acc_steps < 10) {
        acc_steps = 10;
    }

    dec_steps = axis->dec_time / period;
    if (dec_steps < 10) {
        dec_steps = 10;
    }
    //转换倍率
    period *= 1000;

    //保证加减速尽量对齐，需要保证加减数是偶数，由于计算误差，加1能让结果更加逼近设置的时间
    acc_steps++;
    acc_steps >>= 1;
    dec_steps++;
    dec_steps >>= 1;

    //保证是偶次数加减数，能减少计算误差
    //if ((min_acc_step & 0x01) != 0)
    //    min_acc_step++;
    //目标速度不能比开始速度小
    if (target_speed < start_speed) {
        target_speed = start_speed;
    }

    //S加速算法需要先计算出加加速
    //推导过程：
    //S加减速度分为加加速与减加速度两个过程，假定两个过程是对称的，那么总占用时间为加速时间t的一半
    //因此利用积分公式(dv = sadt, vm = (vd - v0) / 2 => a = vm / (2 * (t / 2))可以得到acc = (vd - v0) * 2 / t
    //其中a与acc表示加速度，vd表示目标速度，v0表示开始速度t表示加速时间一半
    //加加速度aac与加速度关系acc为acc = aac * (t / 2)
    //从而公式为aac = (vd - v0) * 4 / (t ^2)
    //其中acc表示加速度，aac表示加加速，vd为目标速度，v0为开始速度，t为总的加速时间，在这算法里它就是加速度步数
    //由于加速度步数要对称就必须保持偶数，加上一些计算偏差以及为了保证电机正常运算必须限制参数
    //因此实际上两个过程在实际上是很难对称的，这就造成了实际加速时间肯定会比设定的长，具体长多
    //跟最大加加速，最大中速度，最小脉冲数，计算偏差，最小加速度步数有关
    cur_aacddc = ((target_speed - axis->start_speed)) / (acc_steps * acc_steps);
    //保证至少1以上的加减速度
    if (cur_aacddc < 1) {
        cur_aacddc = 1;
    }
    //计算加速度会达到的最大值
    cur_max_accdec = cur_aacddc * acc_steps;
    if (cur_max_accdec > axis->max_acc) {
        cur_max_accdec = axis->max_acc;
    }

    //限制加加速度，加加速度过大会导致过冲
    if (cur_aacddc > axis->max_aac) {
        cur_aacddc = axis->max_aac;
    }
    //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
    acc_steps = cur_max_accdec / cur_aacddc;

    if (cur_aacddc == 0) cur_aacddc = 1;



    //开始计算加速表
    mid_speed = (target_speed + axis->start_speed) >> 1;
    ddc_speed = target_speed - ((cur_aacddc * acc_steps * acc_steps) >> 1);
    if (ddc_speed < mid_speed) {
        ddc_speed = mid_speed;
    }
    cur_speed = axis->start_speed;


    //S加速曲线加速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长
        if (cur_speed >= mid_speed) {
            cur_accdec = cur_max_accdec;
            //保证只能进一次
            mid_speed *= 3;
        }

        if (cur_speed >= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (cur_accdec > cur_aacddc) {
                cur_accdec -= cur_aacddc;
            }
            else {
                //保证至少有这个加加速，让速度有机会达到目标速度
                cur_accdec = cur_aacddc;
            }
        }

        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = ((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_ACC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_ACC_NUMBER_TABLE(axis)[index] = (period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value;
            if (AXIS_ACC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_ACC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->acc_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < ddc_speed) {        //还没到开始减速的速度
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }

        }

    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存表大小
    axis->acc_table_size = index;

#ifdef AXIS_USING_ACC_TABLE_ONLY
    if (index > 0) {
        index -= 1;
    }
    axis->dec_table_size = index;
#else

    if (dec_steps != acc_steps) {
        cur_aacddc = ((target_speed - axis->start_speed)) / (dec_steps * dec_steps);
        //保证至少1以上的加减速度
        if (cur_aacddc < 1) {
            cur_aacddc = 1;
        }
        //计算加速度会达到的最大值
        cur_max_accdec = cur_aacddc * dec_steps;
        if (cur_max_accdec > axis->max_acc) {
            cur_max_accdec = axis->max_acc;
        }

        //限制加加速度，加加速度过大会导致过冲
        if (cur_aacddc > axis->max_aac) {
            cur_aacddc = axis->max_aac;
        }
        //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
        dec_steps = cur_max_accdec / cur_aacddc;
        //curMaxAccDec = curAacDdc * decSteps;
        if (cur_aacddc == 0) cur_aacddc = 1;
    }

    //如果减速时间一样，就不重复计算表了
    if (dec_steps == acc_steps) {
        int j = 0;
        while (j < index) {
            AXIS_DEC_REG_TABLE(axis)[j] = AXIS_ACC_REG_TABLE(axis)[j];
            AXIS_DEC_NUMBER_TABLE(axis)[j] = AXIS_ACC_NUMBER_TABLE(axis)[j];
            j++;
        }
        axis->dec_table_size = index;
        if (axis->dec_table_size > 0) {
            axis->dec_table_size -= 1;
        }
        return;
    }

    //开始减速表计算
    index = 0;
    last_reg_value = 0;
    mid_speed = (target_speed + axis->start_speed) >> 1;
    ddc_speed = target_speed - ((cur_aacddc * dec_steps * dec_steps) >> 1);

    if (ddc_speed < mid_speed) {
        ddc_speed = mid_speed;
    }

    cur_speed = axis->start_speed;

    //S加速曲线减速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长
        if (cur_speed >= mid_speed) {
            cur_accdec = cur_max_accdec;
            //保证只可能进一次
            mid_speed *= 3;
        }
        if (cur_speed >= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (cur_accdec > cur_aacddc) {
                cur_accdec -= cur_aacddc;
            }
            else {
                //保证至少有这个加加速，让速度有机会达到目标速度
                cur_accdec = cur_aacddc;
            }
        }

        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = ((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_DEC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_DEC_NUMBER_TABLE(axis)[index] = (period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value;
            if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->dec_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < ddc_speed) {        //还没到开始减速的速度
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }
        }

    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存减速表大小
    axis->dec_table_size = index;
    if (axis->dec_table_size > 0) {
        axis->dec_table_size -= 1;
    }
#endif
}

/**
 * 以定加加速的S曲线方式计算加减速表
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象
 * @param target_speed 目标速度
 */
static void axis_mode2_calc_accdec(axis_object_t *axis, int target_speed) {
    int cur_accdec = 0;  //当前加减速度
    int cur_max_accdec = 0; //当前最大加速度减速度
    int cur_speed = 0;   //当前速度
    int ddc_speed = 0;   //减减速开始速度
    int cur_aacddc = 0;  //当前加加速减减速
    int mid_speed = 0;   //中间速度
    int index = 0;
    int clock = axis->clock;
    int clock_ratio = axis->clock / 1000000;
    int start_speed = axis->start_speed;
    //周期临时值
    int period;
    int reg_Value = 0;
    int last_reg_value = 0;

    period = axis->period;
    //限制加速度周期
    if (period < 1) period = 1;

    //转换倍率
    period *= 1000;

    //目标速度不能比开始速度小
    if (target_speed < start_speed) {
        target_speed = start_speed;
    }





    //开始计算加速表
    mid_speed = (target_speed + axis->start_speed) / 2;
    cur_speed = axis->start_speed;

    cur_aacddc = axis->max_aac;
    cur_max_accdec = axis->max_acc;
    cur_accdec = 0;
    ddc_speed = 0;
    //S加速曲线加速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长

        if (cur_speed >= mid_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (ddc_speed != 0) {   //ddc_speed是为了保证第一次开始减速的时候,加加速不掉下来,从而保证曲线中心至少在上方
                if (cur_accdec > cur_aacddc) {
                    cur_accdec -= cur_aacddc;
                }
                else {
                    //保证至少有这个加加速，让速度有机会达到目标速度
                    cur_accdec = cur_aacddc;
                }
            }
            else {
                ddc_speed = mid_speed;
            }
        }
        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = ((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_ACC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_ACC_NUMBER_TABLE(axis)[index] = (period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value;
            if (AXIS_ACC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_ACC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->acc_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < mid_speed) {
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }
        }
    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存表大小
    axis->acc_table_size = index;

#ifdef AXIS_USING_ACC_TABLE_ONLY
    if (index > 0) {
        index -= 1;
    }
    axis->dec_table_size = index;
#else

    if (dec_steps != acc_steps) {
        cur_aacddc = ((target_speed - axis->start_speed)) / (dec_steps * dec_steps);
        //保证至少1以上的加减速度
        if (cur_aacddc < 1) {
            cur_aacddc = 1;
        }
        //计算加速度会达到的最大值
        cur_max_accdec = cur_aacddc * dec_steps;
        if (cur_max_accdec > axis->max_acc) {
            cur_max_accdec = axis->max_acc;
        }

        //限制加加速度，加加速度过大会导致过冲
        if (cur_aacddc > axis->max_aac) {
            cur_aacddc = axis->max_aac;
        }
        //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
        dec_steps = cur_max_accdec / cur_aacddc;
        //curMaxAccDec = curAacDdc * decSteps;
        if (cur_aacddc == 0) cur_aacddc = 1;
    }

    //如果减速时间一样，就不重复计算表了
    if (axis->max_dec == axis->max_dec) {
        int j = 0;
        while (j < index) {
            AXIS_DEC_REG_TABLE(axis)[j] = AXIS_ACC_REG_TABLE(axis)[j];
            AXIS_DEC_NUMBER_TABLE(axis)[j] = AXIS_ACC_NUMBER_TABLE(axis)[j];
            j++;
        }
        axis->dec_table_size = index;
        if (axis->dec_table_size > 0) {
            axis->dec_table_size -= 1;
        }
        return;
    }

    //开始减速表计算
    index = 0;
    last_reg_value = 0;
    mid_speed = (target_speed + axis->start_speed) / 2;
    ddc_speed = 0;
    cur_accdec = 0;
    cur_max_accdec = axis->max_dec;
    cur_speed = axis->start_speed;

    //S加速曲线减速表计算
    do {
        //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
        //否则会造成波形中间下移，导致减加速流程过长
        if (cur_speed >= mid_speed) {  //减减速需要提前判断，尽量保证流程对称
            //减加速流程
            if (ddc_speed != 0) {
                if (cur_accdec > cur_aacddc) {
                    cur_accdec -= cur_aacddc;
                }
                else {
                    //保证至少有这个加加速，让速度有机会达到目标速度
                    cur_accdec = cur_aacddc;
                }
            }
            else {
                ddc_speed = mid_speed;
            }
        }

        //计算速度
        cur_speed += cur_accdec;
        if (cur_speed > target_speed) cur_speed = target_speed;
        //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
        reg_Value = ((clock) / cur_speed);
        //过滤器重复的加减速点
#ifdef AXIS_IGNORE_REPEAT_STEP
        if (last_reg_value != reg_Value) {
#endif
            last_reg_value = reg_Value;
            AXIS_DEC_REG_TABLE(axis)[index] = reg_Value;
            AXIS_DEC_NUMBER_TABLE(axis)[index] = (period + AXIS_CALC_ROUND(reg_Value)) * clock_ratio / reg_Value;
            if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) {
                AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
            }
#ifdef AXIS_USING_DEBUG
            axis->dec_speed_table[index] = cur_speed;
#endif
            index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
        }
#endif
        if (cur_speed < mid_speed) {        //还没到开始减速的速度
            //加加速流程
            cur_accdec += cur_aacddc;
            if (cur_accdec >= cur_max_accdec) {
                cur_accdec = cur_max_accdec;
            }
        }

    } while (cur_speed < target_speed && (index + 1) < AXIS_MAX_ACCDEC_STEP);

    //保存减速表大小
    axis->dec_table_size = index;
    if (axis->dec_table_size > 0) {
        axis->dec_table_size -= 1;
    }
#endif
}

#endif

/**
 * 计算加减速表的接口
 * 
 * @author LXZ (021720)
 * 
 * @param axis 
 * @param target_speed 
 */
void axis_calc_speed_table(axis_object_t *axis, int target_speed) {
    if (axis->accdec_mode == 1) {
        axis_mode1_calc_accdec(axis, target_speed);
    }
    else if (axis->accdec_mode == 2) {
        axis_mode2_calc_accdec(axis, target_speed);
    }
    else if (axis->accdec_mode == 3) {
        axis_mode3_calc_accdec(axis, target_speed);
    }
    else {
        axis_mode0_calc_accdec(axis, target_speed);
    }
}


/**
 * 轴运动中断函数
 * 
 * @author lxz 
 * 
 * @param handle 
 */
void axis_it_handle(void *handle) {
    axis_object_t *axis = (axis_object_t *)handle;
    unsigned short reg_value = 0;

    axis->position += axis->feed;
    switch (axis->run_mode) {
    case AXIS_MODE_PP:
//		if (axis->feed > 0) {
        if ((axis->position - axis->dec_position) * axis->feed >= 0) { //开始减速的条件
            if (axis->cur_accdec_status != AXIS_ACCDEC_STATUS_DEC) {
                axis->cur_accdec_status = AXIS_ACCDEC_STATUS_DEC;
                axis->cur_speed_index = axis->dec_speed_index;
                axis->accdec_count = AXIS_DEC_NUMBER_TABLE(axis)[axis->cur_speed_index];
            }
            else if (axis->accdec_count > 1) { //减速脉冲计数
                axis->accdec_count--;
            }
            else if (axis->cur_speed_index > 0) { //切换速度
                axis->cur_speed_index--;
                axis->accdec_count = AXIS_DEC_NUMBER_TABLE(axis)[axis->cur_speed_index];
            }
            reg_value = AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index];
        }
        else {
            axis->cur_accdec_status = AXIS_ACCDEC_STATUS_ACC;
            if (axis->accdec_count > 1) { //加速脉冲计数
                axis->accdec_count--;
            }
            else if (axis->cur_speed_index < axis->acc_speed_index) { //切换速度
                axis->cur_speed_index++;
                axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
            }
            reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
        }
        //到达位置立刻停止
        if ((axis->position - axis->target_position) * axis->feed >= 0) {
            AXIS_OFF(axis);
        }

        break;
#ifdef AXIS_PP_TASK_NUMBER
    case AXIS_MODE_PP_TASKS:
        if ((axis->position - axis->tasks[axis->cur_task_index].dec_position) * axis->feed >= 0) {
            if (axis->cur_task_index + 1 < axis->task_number) {
                if (axis->cur_accdec_status != AXIS_ACCDEC_STATUS_DEC) {
                    axis->cur_accdec_status = AXIS_ACCDEC_STATUS_DEC;
                    axis->cur_speed_index = axis->tasks[axis->cur_task_index].dec_speed_index;
                    axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
                }
                else if (axis->accdec_count > 1) {
                    axis->accdec_count--;
                }
                else if (axis->cur_speed_index > axis->tasks[axis->cur_task_index + 1].dst_speed_index) {
                    axis->cur_speed_index--;
                    axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
                }
                reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
            }
            else {
                if (axis->cur_accdec_status != AXIS_ACCDEC_STATUS_DEC) {
                    axis->cur_accdec_status = AXIS_ACCDEC_STATUS_DEC;
                    axis->cur_speed_index = axis->tasks[axis->cur_task_index].dec_speed_index;
                    axis->accdec_count = AXIS_DEC_NUMBER_TABLE(axis)[axis->cur_speed_index];
                }
                else if (axis->accdec_count > 1) {
                    axis->accdec_count--;
                }
                else if (axis->cur_speed_index > 0) {
                    axis->cur_speed_index--;
                    axis->accdec_count = AXIS_DEC_NUMBER_TABLE(axis)[axis->cur_speed_index];
                }
                reg_value = AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index];

            }

        }
        else {
            axis->cur_accdec_status = 0;
            if (axis->accdec_count > 1) { //加速脉冲计数
                axis->accdec_count--;
            }
            else if (axis->cur_speed_index < axis->tasks[axis->cur_task_index].dst_speed_index) { //切换速度
                axis->cur_speed_index++;
                axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
            }

            if (axis->cur_speed_index == axis->tasks[axis->cur_task_index].dst_speed_index &&
                axis->accdec_count == 0) {
                reg_value = axis->tasks[axis->cur_task_index].reg_value;
            }
            else {
                reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
            }

        }
        if ((axis->position - axis->tasks[axis->cur_task_index].target_position) * axis->feed >= 0) {
            axis->cur_task_index++;
            axis->cur_accdec_status = 0;
            if (axis->cur_task_index >= axis->task_number) {
                AXIS_OFF(axis);
            }
        }

        break;
#endif

    case AXIS_MODE_PV:
        axis->cur_accdec_status = AXIS_ACCDEC_STATUS_ACC;
        if (axis->accdec_count > 1) {
            axis->accdec_count--;
        }
        else if (axis->cur_speed_index < axis->acc_speed_index) {
            axis->cur_speed_index++;
            axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
        }
        else if (axis->cur_speed_index > axis->acc_speed_index) {
            axis->cur_speed_index--;
            axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
        }
        else {
            axis->accdec_count = 0;
        }
        if (axis->accdec_count == 0 && axis->cur_speed_index == axis->acc_speed_index) {
            reg_value = axis->target_speed_reg;
        }
        else {
            reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
        }

        break;
    case AXIS_MODE_INTERP:
        if (axis->accdec_count > 1) {
            axis->accdec_count--;
        }
//		else if (axis->interp_feed != 0) {
//			//检测是否有设置新的进给任务
//			if (axis->interp_feed > 0) {
//				axis->accdec_count = axis->interp_feed;
//				axis_cw(axis);
//			}
//			else
//			{
//				axis->accdec_count = -axis->interp_feed;
//				axis_ccw(axis);
//			}
//			axis->interp_feed = 0;
//			axis->dst_period = axis->interp_reg;
//		}
        else {
            //没有就需要轴停止
            AXIS_OFF(axis);
        }
        reg_value = axis->dst_period;
        break;
    case AXIS_MODE_STOP:

        //由于允许加减速表不对称，因此需要保护不要溢出
        if (axis->cur_accdec_status != AXIS_ACCDEC_STATUS_DEC) {
            axis->cur_accdec_status = AXIS_ACCDEC_STATUS_DEC;
            axis->cur_speed_index = axis->dec_speed_index;
            axis->accdec_count = AXIS_DEC_NUMBER_TABLE(axis)[axis->cur_speed_index];
        }
        else if (axis->accdec_count > 1) {
            axis->accdec_count--;
        }
        else {
            //计数一定脉冲后切换速度
            if (axis->cur_speed_index > 0) axis->cur_speed_index--;
            else AXIS_OFF(axis);
            axis->accdec_count = AXIS_DEC_NUMBER_TABLE(axis)[axis->cur_speed_index];
        }
        reg_value = AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index];
        break;
    case AXIS_MODE_EMGSTOP:
        axis->home_step = 0;
        AXIS_OFF(axis);
        break;
    }
    
    if (axis->outputs.homing) { //回零时中断处理信号
        switch (axis->home_method) {    //根据方法不同回零使能判断部位也不同
        case 0:
            if (axis->home_step == 6) { //回零方式0需要在这里处理信号
              
                if (AXIS_GET_HOME_SGN(axis) && axis->inputs.hw_home_sgn) {  //回零信号滤波
                    if (++axis->home_sgn_count >= axis->home_sgn_filter) 
                    {  //确定信号
                        if (axis->inputs.home_encode_sgn_enable) {  //使能了编码器回零信号
                            axis->home_step++;
                        }
                        else {
                            axis->home_step += 2;
                            AXIS_OFF(axis);

                        }
                    }
                }
            }
            else if (axis->home_step == 7) {
                if (AXIS_GET_HOME_SGN(axis)) {  //回零信号滤波
                    if (++axis->home_sgn_count >= axis->home_sgn_filter) {  //确定信号
                        if (axis->inputs.home_encode_sgn_enable) {  //启动编码器回零信号滤波
                            axis->home_step++;
                            axis->home_encoder_sgn_count = 0;
                        }
                        else {
                            axis->home_step += 2;
                            AXIS_OFF(axis);

                        }
                    }
                }
                if (AXIS_GET_HOME_ENCODER_SGN(axis)) {
                    if (++axis->home_encoder_sgn_count >= axis->home_encoder_sgn_filter) {  //确定编码器回零信号
                        axis->home_step++;
                        AXIS_OFF(axis);
                    }
                }
            }
            break;
        case 2:
            if (axis->home_step == 3) { //回零方式0需要在这里处理信号
                if (AXIS_GET_HOME_SGN(axis)) {  //回零信号滤波
                    if (axis->inputs.home_encode_sgn_enable) {  //启动编码器回零信号滤波
                        axis->home_step++;
                        axis->home_encoder_sgn_count = 0;
                    }
                    else {
                        if (++axis->home_sgn_count >= axis->home_sgn_filter) {  //确定信号
                            axis->home_step += 2;
                            AXIS_OFF(axis);
                        }
                    }
                }
            }
            else if (axis->home_step == 4) {
                if (AXIS_GET_HOME_ENCODER_SGN(axis)) {  //编码器回零信号滤波
                    if (++axis->home_encoder_sgn_count >= axis->home_encoder_sgn_filter) {  //确定编码器回零信号
                        axis->home_step++;
                        AXIS_OFF(axis);
                    }
                }
            }
            break;
        }
    }
    AXIS_UPDATE_PERION(axis, reg_value);
    if (axis->handle != 0) {
        axis->handle();
    }

}


/**
 * 以目标速度移动到指定位置
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象 
 * @param rel
 *  		  位置参考系，0表示相对坐标，1表进绝对值坐标
 * @param position 目标位置
 * @param target_speed 目标速度 
 *  
 * @return 
 *         返回0表示正常启动，返回-1表示目标超过正软件限位，返回-2表示目标超过负软件限位，
 *         返回-3表示目标进给为0
 */
int axis_pp(axis_object_t *axis, int rel, int position, int target_speed) {
    //允许加速表可执行到的位置
    int acc_index = 0;
    //当前减速表可执行到的位置
    int dec_index = 0;
    //开始减速的位置
    int dec_position = 0;

    //齿轮比转换
    if (axis->inputs.ratio_enable) {
        position = axis_position_to_pluse(axis, position);
    }

    //计算目标停止位置
    axis->target_position = position;
    if (rel == 0) {
        axis->target_position += axis->position;
    }
    else {
        position -= axis->position;
    }

    if (position == 0) {    //总进给为0时不动
//      axis->outputs.sw_feed_zero_alarm = 1;
        return -3;
    }


    //换算成绝对值
    if (position < 0) {
        position = -position;
    }

    //计算进给方向
    if (axis->target_position > axis->position) {
        axis_cw(axis);
    }
    else {
        axis_ccw(axis);
    }

    if (axis->inputs.sw_lmt_enable) {   //使能了软件限位
        if (axis->target_position > axis->sw_positive_limit && axis->feed > 0) {  //正向时目标位置比软件限位大的时候不走
            axis->outputs.sw_positive_pre_alarm = 1;
            return -1;
        }
        else if (axis->target_position < axis->sw_negative_limit && axis->feed < 0) { //反向时目标位置比软件限位小的时候不走
            axis->outputs.sw_negative_pre_alarm = 1;
            return -2;
        }
    }

#if defined(AXIS_SPEED_UNIT)
    axis_calc_speed_table(axis, target_speed * axis->speed_unit);
#else
    axis_calc_speed_table(axis, target_speed);
#endif
    dec_index = 0;
    dec_position += AXIS_DEC_NUMBER_TABLE(axis)[dec_index];

    do {
        //加速区域判断
        //当当前的加速时速度比当前的减速时速度相等或者大的时候，可以进行一次减速
        //这是为了保证加减速度差在可以接受的范围内
        //这样也就可以让加减时间更加的灵活

        if (AXIS_ACC_REG_TABLE(axis)[acc_index] < AXIS_DEC_REG_TABLE(axis)[dec_index] &&
            dec_index < axis->dec_table_size - 1) {
            //如果不能进行减加速，退出
            if (position < AXIS_DEC_NUMBER_TABLE(axis)[dec_index]) break;
            dec_index++;
            position -= AXIS_DEC_NUMBER_TABLE(axis)[dec_index];
            dec_position += AXIS_DEC_NUMBER_TABLE(axis)[dec_index];
        }
        else if (acc_index < (axis->acc_table_size - 1)) {
            //如果已经不能加速就退出
            if (position < AXIS_ACC_NUMBER_TABLE(axis)[acc_index] * AXIS_LOCATION_SMOOTH) break;
            position -= AXIS_ACC_NUMBER_TABLE(axis)[acc_index];
            acc_index++;
        }
        else {
            break;
        }

    } while (position > 0);

    axis->acc_speed_index = acc_index;
    axis->dec_speed_index = dec_index;

    axis->dec_position = axis->target_position - dec_position * axis->feed;

    //让中断切换到定位模式
    axis->run_mode = AXIS_MODE_PP;
    axis_start(axis);
    return 0;
}


/**
 * 以目标速度执行连续运动
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象
 * @param dir 运动方向，0为反转，1为正转
 * @param target_speed 目标速度 
 */
void axis_pv(axis_object_t *axis, int dir, int target_speed) {
    if (dir == 0) axis_ccw(axis);
    else axis_cw(axis);
#if defined(AXIS_SPEED_UNIT)
    axis_calc_speed_table(axis, target_speed * axis->speed_unit);
#else
    axis_calc_speed_table(axis, target_speed);
#endif
    axis->acc_speed_index = axis->acc_table_size - 1;
    axis->target_speed_reg = AXIS_ACC_REG_TABLE(axis)[axis->acc_speed_index];
    //让中断切换到连续运动模式
    axis->run_mode = AXIS_MODE_PV;

    axis_start(axis);
}

/**
 * 轴插补任务
 * 
 * @author LXZ (033020)
 * 
 * @param axis 
 * @param position		进给的脉冲个数，有方向
 * @param period	    周期，用于速度 
 */
void axis_interp(axis_object_t *axis, int position, int period) {

    while (AXIS_IS_ON(axis));   //为了防止漏周期功能，这里需要进行死等

    axis->run_mode = AXIS_MODE_INTERP;

    if (position > 0)  {
        axis_cw(axis);
        axis->accdec_count = position;
    }
    else {
        axis_ccw(axis);
        axis->accdec_count = -position;
    }
    axis->dst_period = period;
    axis->interp_feed = position;
    AXIS_SET_PERION(axis, period);
    AXIS_ON(axis);
}

#ifdef AXIS_PP_TASK_NUMBER
/**
 * 初始化轴任务缓冲，并设置此时的方向
 * 
 * @author LXZ (031420)
 * 
 * @param axis 轴对象
 * @param dir  期望运动的方向
 */
void axis_init_pp_task(axis_object_t *axis, int dir) {
    axis->task_number = 0;
    memset(axis->tasks, 0, sizeof(axis->tasks));

    if (dir) {
        axis_cw(axis);
    }
    else {
        axis_ccw(axis);
    }
}
/**
 * 添加进给值，该进给值都是绝对进给值，与方向与当前坐标无关，方向在初始化任务缓冲的时候就已经设置了
 * 
 * @author LXZ (031420)
 * 
 * @param axis 	轴对象
 * @param feed  进给值
 * @param speed 期望速度
 */
void axis_add_pp_task(axis_object_t *axis, int feed, int speed) {
    if (feed <= 0) return;
    if (axis->task_number < AXIS_PP_TASK_NUMBER && feed > 0) {
        axis->tasks[axis->task_number].position = axis_position_to_pluse(axis, feed);
        axis->tasks[axis->task_number].speed = speed;
        axis->task_number++;
    }
}
/**
 * 启动多段速定位运动模式
 * 
 * @author LXZ (031420)
 * 
 * @param axis 
 *  
 * @return 
 *         返回0表示正常启动，返回-1表示目标超过正软件限位，返回-2表示目标超过负软件限位,
 *         返回-3表示为有某段进给为0
 */
int axis_start_pp_task(axis_object_t *axis) {
    int speed;
    int i;
    int posi1;  //上一个位置
    int posi2;  //当前计算位置
    int feed = 0;                   //进给量
    int cur_speed_index = 0;        //当前速度
    int dst_speed_index = 0;        //目标速度
    int dec_speed_index = 0;             //
    int dec_position = 0;           //开始减速的位置
    if (axis->task_number == 0) return -3;

    //计算最高速度
    for (i = 0, speed = 0; i < axis->task_number; i++) {
        if  (speed < axis->tasks[i].speed) speed = axis->tasks[i].speed;

    }
#if defined(AXIS_SPEED_UNIT)
    speed *= axis->speed_unit;
#endif

    if (speed < axis->start_speed) {
        speed = axis->start_speed;
    }

    axis_calc_speed_table(axis, speed);



    posi1 = 0;
    for (i = 0; i < axis->task_number; i++) {

#if defined(AXIS_SPEED_UNIT)
        speed = axis->tasks[i].speed * axis->speed_unit;
#else
        speed = axis->tasks[i].speed;
#endif
        if (speed < axis->start_speed) {
            speed = axis->start_speed;
        }
        axis->tasks[i].reg_value = axis->clock / speed;
        dst_speed_index = 0;
        do {
            if (AXIS_ACC_REG_TABLE(axis)[dst_speed_index] > axis->tasks[i].reg_value) {
                dst_speed_index++;
            }
            else if (AXIS_ACC_REG_TABLE(axis)[dst_speed_index] < axis->tasks[i].reg_value) {
                if (dst_speed_index > 0) dst_speed_index--;
                break;
            }
            else {
                break;
            }
        } while ((dst_speed_index + 1) < axis->acc_table_size);
        axis->tasks[i].dst_speed_index = dst_speed_index;

        //计算目标位置
        posi1 += axis->tasks[i].position;
        axis->tasks[i].target_position = posi1 * axis->feed + axis->position;
        if (axis->inputs.sw_lmt_enable) {
            if (axis->tasks[i].target_position > axis->sw_positive_limit && (axis->feed > 0)) {
                axis->outputs.sw_positive_pre_alarm = 1;
                return -1;
            }
            else if (axis->tasks[i].target_position < axis->sw_negative_limit && (axis->feed < 0)) {
                axis->outputs.sw_negative_pre_alarm = 1;
                return -2;
            }
        }

        if (axis->tasks[i].target_position == axis->position) {
//          axis->outputs.sw_feed_zero_alarm = 1;
            return -3;
        }
    }

    posi2 = axis->tasks[i].target_position;
    //进行减速位置安排
    dst_speed_index = 0;
    for (i = 0; i < axis->task_number; i++) {
        //计算进给量
        posi1 = posi2;
        posi2 = axis->tasks[i].target_position;
        //cur_speed_index = dst_speed_index;
        dst_speed_index = axis->tasks[i].dst_speed_index;

        feed = axis->tasks[i].position;

        if (i + 1 == axis->task_number) {
            //最后一个流程需要涉及到减停操作
            //由上一个速度计算减速位置
            dec_speed_index = 0;
            dec_position = 0;
            dec_position += AXIS_DEC_NUMBER_TABLE(axis)[dec_speed_index];
            do {
                //加速区域判断
                //当当前的加速时速度比当前的减速时速度相等或者大的时候，可以进行一次减速
                //这是为了保证加减速度差在可以接受的范围内
                //这样也就可以让加减时间更加的灵活


                if (cur_speed_index > dst_speed_index) {                   //如果当前速度比期望速度大，需要先降速再打算，
                                                                           // 然而这样的机会在前面运算正常的情况下是很少出现的
                    if (feed < AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index]) break;
                    feed -= AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index];
                    cur_speed_index--;
                }
                else if (AXIS_ACC_REG_TABLE(axis)[cur_speed_index] < AXIS_DEC_REG_TABLE(axis)[dec_speed_index] &&
                         dec_speed_index < axis->dec_table_size - 1) {         //当前速度在期望速度以下先保证减速位置
                    //如果不能进行减加速，退出
                    if (feed < AXIS_DEC_NUMBER_TABLE(axis)[dec_speed_index]) break;
                    dec_speed_index++;
                    feed -= AXIS_DEC_NUMBER_TABLE(axis)[dec_speed_index];
                    dec_position += AXIS_DEC_NUMBER_TABLE(axis)[dec_speed_index];
                }
                else if (cur_speed_index < dst_speed_index) {           //如果当前速度是在期望速度以下，就需要有加速环节
                    //如果已经不能加速就退出
                    if (feed < AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index]) break;
                    feed -= AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index];
                    cur_speed_index++;
                }
                else {
                    break;
                }

            } while (feed > 0);
            //设置停止位置
            axis->tasks[i].dec_position = posi2 - axis->feed * dec_position;
            axis->tasks[i].dec_speed_index = dec_speed_index;
        }
        else {
            //当前段的减速其实就是下一段的匀速，这样才能保证进入下一段的时候速度能保证是期望的速度
            dec_speed_index = axis->tasks[i + 1].dst_speed_index;
            dec_position = 0;
            if (dec_speed_index < dst_speed_index) {
                //可以完全忽视减速部分，因此减速位置为0
                dec_position += AXIS_ACC_NUMBER_TABLE(axis)[dec_speed_index];
            }
            do {
                //加速区域判断
                //当当前的加速时速度比当前的减速时速度相等或者大的时候，可以进行一次减速
                //这是为了保证加减速度差在可以接受的范围内
                //这样也就可以让加减时间更加的灵活
                //为了保证效果，当下一阶段速比当前阶段高的时候，减速段不进行操作，因为本来就是以
                //保证阶段速度不能高于指定速度的目的来做这个功能的
                if (cur_speed_index > dst_speed_index) {      //如果当前速度比期望速度大，需要先降速再打算，
                                                              // 然而这样的机会在前面运算正常的情况下是很少出现的
                    if (feed < AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index]) break;
                    feed -= AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index];
                    cur_speed_index--;
                    dec_speed_index = dst_speed_index;
                }
                else if (cur_speed_index > dec_speed_index && dec_speed_index < dst_speed_index) {
                    //如果当前速度比减速大，需要减速过程
                    if (feed < AXIS_ACC_NUMBER_TABLE(axis)[dec_speed_index]) break;
                    dec_speed_index++;
                    feed -= AXIS_ACC_NUMBER_TABLE(axis)[dec_speed_index];
                    dec_position += AXIS_ACC_NUMBER_TABLE(axis)[dec_speed_index];
                }
                else if (cur_speed_index < dst_speed_index) { //如果当前速度比期望速度小，需要进行加速
                    if (feed < AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index]) break;
                    feed -= AXIS_ACC_NUMBER_TABLE(axis)[cur_speed_index];
                    cur_speed_index++;
                }
                else {
                    break;
                }

            } while (feed > 0);
            if (dec_speed_index > dst_speed_index) {
                //可以完全忽视减速部分，因此减速位置为0
                dec_speed_index  = cur_speed_index;
            }
            //设置停止位置
            axis->tasks[i].dec_position = posi2 - axis->feed * dec_position;
            axis->tasks[i].dec_speed_index = dec_speed_index;
            //修正下次计算的开始速度,如果下次的速度比这次小，减速后也应该是进入的速度
            if (dst_speed_index > axis->tasks[i + 1].dst_speed_index) {
                cur_speed_index = axis->tasks[i + 1].dst_speed_index;
            }

        }
    }
    axis->cur_task_index = 0;
    axis->run_mode = AXIS_MODE_PP_TASKS;
    axis_start(axis);
    return 0;
}
#endif		//#ifdef AXIS_TASK_NUMBER

/**
 * 减速停止
 * 
 * @author LXZ (021720)
 * 
 * @param axis 
 */
void axis_stop(axis_object_t *axis) {
    if (axis->run_mode != AXIS_MODE_STOP) {
        if (axis->cur_accdec_status != AXIS_ACCDEC_STATUS_DEC) {
            int cur_acc_index = axis->cur_speed_index;
            int star_dec_index = 0;
            //查找开始减速的点
            unsigned short number = AXIS_ACC_NUMBER_TABLE(axis)[cur_acc_index];
            do {
                //当当前开始减速的点比当前开始加速的点速度大或者等于的时候，说明已经找到点了
                if (AXIS_DEC_NUMBER_TABLE(axis)[star_dec_index] >= number) break;
                star_dec_index++;
            } while (star_dec_index < (axis->dec_table_size - 1));
            //减速点开始位置必须速度比当前速度低，因此需要减一，但是如果是最高速，却是不需要的
            if (star_dec_index > 0 && star_dec_index != ((axis->dec_table_size - 1))) star_dec_index--;

            axis->dec_speed_index = star_dec_index;
            //让中断切换到减速停止模式
            axis->run_mode = AXIS_MODE_STOP;
        }
    }
}



/**
 * 非停止的运行模式下保持当前方向运行到指定位置停止
 * 支持PV=>PP与PP=>PP
 * 当位置不够减速停止时会执行减速停止，所以需要保证有足够位置提前调用
 * 
 * @author LXZ (021720)
 * 
 * @param axis 轴对象
 * @param position 
 */
void axis_stop_at(axis_object_t *axis, int position) {
    int cur_position = axis->position;
    int acc_index = 0;
    int dec_index = 0;
    //开始减速的位置
    int dec_position = 0;
    //停止状态下调用该函数无效
    if (AXIS_IS_RUNNING(axis) == 0) return;
    //齿轮比转换
    if (axis->inputs.ratio_enable) {
        position = axis_position_to_pluse(axis, position);
    }
    //如果要停的位置已经超过了，直接减速停止
    if (axis->feed > 0) {
        if (cur_position > position) {
            axis_stop(axis);
            return;
        }
    }
    else if (cur_position <= position) {
        axis_stop(axis);
        return;
    }

    //设置要停止的目标位置
    axis->target_position = position;
    //计算要
    position -= axis->position;
    if (position < 0) {
        position = -position;
    }
    //计算减速位置与开始减速的减速表位置


    if (axis->cur_accdec_status == AXIS_ACCDEC_STATUS_DEC) {   //正在减速状态下，先查找相匹配的加速点
        acc_index = 0;
        dec_index = axis->cur_speed_index;
        while (AXIS_ACC_REG_TABLE(axis)[acc_index] > AXIS_DEC_REG_TABLE(axis)[dec_index]) {
            acc_index++;
        }

    }
    else { //直接获取当前加速点
        acc_index = axis->cur_speed_index;
    }

    dec_index = 0;
    dec_position += AXIS_DEC_NUMBER_TABLE(axis)[dec_index];

    do {

        if (AXIS_ACC_REG_TABLE(axis)[acc_index] < AXIS_DEC_REG_TABLE(axis)[dec_index] &&
            dec_index < (axis->dec_table_size - 1)) { //计算指定速度下减速表开始位置

            if (position < AXIS_DEC_NUMBER_TABLE(axis)[dec_index]) //剩余脉冲数不足
                break;

            //计算剩余脉冲与减速点
            dec_index++;
            position -= AXIS_DEC_NUMBER_TABLE(axis)[dec_index];
            dec_position += AXIS_DEC_NUMBER_TABLE(axis)[dec_index];
        }
        else if (acc_index < (axis->acc_table_size - 1)) {         //允许继续加速
            if (position < AXIS_ACC_NUMBER_TABLE(axis)[acc_index] * AXIS_LOCATION_SMOOTH) //剩余脉冲不足
                break;
            //计算剩余脉冲
            position -= AXIS_ACC_NUMBER_TABLE(axis)[acc_index];
            acc_index++;
        }
        else {
            break;
        }
    } while (position > 0);

    //开始减速的减速表位置
    axis->dec_speed_index = dec_index;
    //计算减速位置
    axis->dec_position = axis->target_position - dec_position * axis->feed;
    if (axis->cur_accdec_status == AXIS_ACCDEC_STATUS_DEC) {
        if (dec_index < axis->cur_speed_index) {
            //由于当前要求的减速开始速度比当前速度要小，需要触发速度跳变
            axis->cur_accdec_status = AXIS_ACCDEC_STATUS_MID;
        }
    }
    //切换运行模式
    axis->run_mode = AXIS_MODE_PP;
}


/**
 * PV模式允许在设定的目标速度范围内进行变速
 * 该接口只有在PV模式下才有用
 * 
 * @author LXZ (021820)
 * 
 * @param axis 	轴对象
 * @param speed 要变化的速度
 */
void axis_pv_change_speed(axis_object_t *axis, int speed) {
    unsigned short reg;
    unsigned short index = 0;
#if AXIS_SPEED_UNIT
    speed *= axis->speed_unit;
#endif
    if (speed < axis->start_speed) {
        speed = axis->start_speed;
    }
    reg = axis->clock / speed;
    while (index < axis->acc_table_size) {
        if (reg <= AXIS_ACC_REG_TABLE(axis)[index]) {
            index++;
        }
        else {
            break;
        }
    }

    axis->acc_speed_index = index;
    axis->target_speed_reg = reg;
}



/**
 * 减速停止所有轴的动作，包括停止回零的操作
 * 
 * @author lxz (2019/6/16/周日)
 * 
 * @param axis 
 */
void axis_stop_all(axis_object_t *axis) {
    axis_stop(axis);
    axis->home_step = 0;
}

/**
 * 以急停的方式去停止
 * 
 * @author lxz 
 * 
 * @param axis 
 */
void axis_emgstop(axis_object_t *axis) {
    axis->home_step = 0;
    axis->run_mode = AXIS_MODE_EMGSTOP;
    AXIS_OFF(axis);
}


/**
 * 轴启动
 * 
 * @author lxz 
 * 
 * @param axis 
 */
void axis_start(axis_object_t *axis) {
    axis->cur_speed_index = 0;
    axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
    axis->cur_accdec_status = 0;
    AXIS_SET_PERION(axis, AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index]);
    AXIS_ON(axis);
    axis->outputs.on = AXIS_IS_ON(axis) != 0;
}

/**
 * 设置轴的参数
 * 
 * @author LXZ (031420)
 * 
 * @param axis 轴对象 
 * @param cmd  设置的对象
 * @param argv 设置的参数
 * 
 */
void axis_set_parameter(axis_object_t *axis, int cmd, int argv) {
    switch (cmd) {
    case AXIS_AXIS_NO:
        axis->axis_no = argv;
        break;
    case AXIS_POSITION:
        axis->position = argv;
        break;
    case AXIS_CLOCK:
        axis->clock = argv;
        AXIS_SET_CLOCK(axis, axis->clock);
        break;
    case AXIS_PERIOD:
        axis->period = argv;
        break;
    case AXIS_MIN_STEP:
        axis->min_steps = argv;
        break;
#if defined(AXIS_SPEED_UNIT)
    case AXIS_SPEED_UNIT:
        axis->speed_unit = argv;
        break;
#endif
    case AXIS_ACCDEC_MODE:
        axis->accdec_mode = argv;
        break;
    case AXIS_ACC_TIME:
        axis->acc_time = argv;
        break;
    case AXIS_DEC_TIME:
        axis->dec_time = argv;
        break;
    case AXIS_AAC_LMT:
        axis->max_aac = argv;
        break;
    case AXIS_ACC_LMT:
        axis->max_acc = argv;
        break;
    case AXIS_DEC_LMT:
        axis->max_dec = argv;
        break;
    case AXIS_START_SPEED:
#if defined(AXIS_SPEED_UNIT)
        argv *= axis->speed_unit;
#endif
        axis->start_speed = argv;
        break;
    case AXIS_HOME_SPACE:
        axis->home_space = argv;
        break;
    case AXIS_HOME_OFFSET:
        axis->home_offset = argv;
        break;
    case AXIS_HOME_FIND_SPEED:
        axis->home_find_speed = argv;
        break;
    case AXIS_HOME_HIGH_SPEED:
        axis->home_high_speed = argv;
        break;
    case AXIS_HOME_LOW_SPEED:
        axis->home_low_speed = argv;
        break;
    case AXIS_HOME_HIGH_SPEED_ENABLE:
        if (argv) {
            axis->inputs.home_high_speed_enable = 1;
        }
        else {
            axis->inputs.home_high_speed_enable = 0;
        }
        break;
    case AXIS_HOME_SGN:
        axis->inputs.hw_home_sgn = argv;
        break;
    case AXIS_HOME_SLOW_SGN:
        axis->inputs.hw_slow_lmt = argv;
        break;
    case AXIS_ERROR_SGN:
        axis->inputs.alarm_sgn = argv;
        break;
    case AXIS_HW_POSI_SGN:
        axis->inputs.hw_positive_lmt = argv;
        break;
    case AXIS_HW_NEGA_SGN:
        axis->inputs.hw_negative_lmt = argv;
        break;

    case AXIS_DIR_LEVEL:
        if (argv) {
            axis->inputs.dir_reverse = 1;
        }
        else {
            axis->inputs.dir_reverse = 0;
        }
        break;
    case AXIS_POSI_SOFT_LMT:               //正软件限位
        if (axis->inputs.ratio_enable) {
            axis->sw_positive_limit = axis_position_to_pluse(axis, argv);
        }
        else {
            axis->sw_positive_limit = argv;
        }
        break;
    case AXIS_NEGA_SOFT_LMT:                //负软件限位
        if (axis->inputs.ratio_enable) {
            axis->sw_negative_limit = axis_position_to_pluse(axis, argv);
        }
        else {
            axis->sw_negative_limit = argv;
        }
        break;
    case AXIS_CYCLE_LENGTH:            //一圈距离
//        axis->cycle_length = argv;
        break;
    case AXIS_CYCLE_PLUSE:             //一圈脉冲数
//        axis->cycle_pluse = argv;
        break;
    case AXIS_REAL_POSITION:          //针对现实单位的位置
        axis->position = axis_position_to_pluse(axis, argv);
        break;
    case AXIS_CURRENT_DIR:                //设置当前方向
        if (argv) {
            axis_cw(axis);
        }
        else {
            axis_ccw(axis);
        }
        break;
    case AXIS_RATIO_ENABLE:
        if (argv) {
            axis->inputs.ratio_enable = 1;
        }
        else {
            axis->inputs.ratio_enable = 0;
        }
        break;
    case AXIS_ENABLE_SGN_LEVEL:
        if (argv) {
            axis->inputs.enable_sgn_level = 1;
        }
        else {
            axis->inputs.enable_sgn_level = 0;
        }
        break;
    }
}


/**
 * 设置轴的参数
 * 
 * @author LXZ (031420)
 * 
 * @param axis 轴对象 
 * @param cmd  设置的对象
 * @param argv 设置的参数
 * 
 */
int axis_get_parameter(axis_object_t *axis, int cmd) {
    int argv = 0;
    switch (cmd) {
    case AXIS_AXIS_NO:
        argv = axis->axis_no;
        break;
    case AXIS_POSITION:
        argv = axis->position;
        break;
    case AXIS_CLOCK:
        argv = axis->clock;
        break;
    case AXIS_PERIOD:
        argv = axis->period;
        break;
    case AXIS_MIN_STEP:
        argv = axis->min_steps;
        break;
#if defined(AXIS_SPEED_UNIT)
    case AXIS_SPEED_UNIT:
        argv = axis->speed_unit;
        break;
#endif
    case AXIS_ACCDEC_MODE:
        argv = axis->accdec_mode;
        break;
    case AXIS_ACC_TIME:
        argv = axis->acc_time;
        break;
    case AXIS_DEC_TIME:
        argv = axis->dec_time;
        break;
    case AXIS_AAC_LMT:
        argv = axis->max_aac;
        break;
    case AXIS_ACC_LMT:
        argv = axis->max_acc;
        break;
    case AXIS_DEC_LMT:
        argv = axis->max_dec;
        break;
    case AXIS_START_SPEED:
        argv = axis->start_speed;
        break;
    case AXIS_HOME_SPACE:
        argv = axis->home_space;
        break;
    case AXIS_HOME_OFFSET:
        argv = axis->home_offset;
        break;
    case AXIS_HOME_FIND_SPEED:
        argv = axis->home_find_speed;
        break;
    case AXIS_HOME_HIGH_SPEED:
        argv = axis->home_high_speed;
        break;
    case AXIS_HOME_LOW_SPEED:
        argv = axis->home_low_speed;
        break;
    case AXIS_HOME_HIGH_SPEED_ENABLE:
        if (axis->inputs.home_high_speed_enable) {
            argv = 1;
        }
        break;
    case AXIS_HOME_SGN:
        argv = axis->inputs.hw_home_sgn;
        break;
    case AXIS_HOME_SLOW_SGN:
        argv = axis->inputs.hw_slow_lmt;
        break;
    case AXIS_ERROR_SGN:
        argv = axis->inputs.alarm_sgn;
        break;
    case AXIS_HW_POSI_SGN:
        argv = axis->inputs.hw_positive_lmt;
        break;
    case AXIS_HW_NEGA_SGN:
        argv = axis->inputs.hw_negative_lmt;
        break;
    case AXIS_DIR_LEVEL:
        if (axis->inputs.dir_reverse) {
            argv = 1;
        }
        break;
    case AXIS_POSI_SOFT_LMT:               //正软件限位
        if (axis->inputs.ratio_enable) {
            argv = axis_pluse_to_position(axis, axis->sw_positive_limit);
        }
        else {
            argv = axis->sw_positive_limit;
        }
        break;
    case AXIS_NEGA_SOFT_LMT:                //负软件限位
        if (axis->inputs.ratio_enable) {
            argv = axis_pluse_to_position(axis, axis->sw_negative_limit);
        }
        else {
            argv = axis->sw_negative_limit;
        }
        break;
    case AXIS_CYCLE_LENGTH:            //一圈距离
        argv = axis->cycle_length;
        break;
    case AXIS_CYCLE_PLUSE:             //一圈脉冲数
        argv = axis->cycle_pluse;
        break;
    case AXIS_REAL_POSITION:          //针对现实单位的位置
        argv = axis_pluse_to_position(axis, axis->position);
        break;
    case AXIS_CURRENT_DIR:                //设置当前方向
        argv = axis->outputs.dir;
        break;
    case AXIS_RATIO_ENABLE:
        argv = axis->inputs.ratio_enable;
        break;
    case AXIS_ENABLE_SGN_LEVEL:
        argv = axis->inputs.enable_sgn_level;
        break;
    }
    return argv;
}


/**
 * 回零方式1
 * 
 * @author lxz (2019/6/10/周一)
 * 
 * @param axis 
 */
void axis_home_method1(axis_object_t *axis) {
    switch (axis->home_step) {
    case 1:
        //负限位或者零点有信号 ，需要向前运动
        if (axis->inputs.hw_home_sgn) {
            //先是离开一定的距离
            axis->home_step = 4;
        }
        else {
            if (axis->inputs.hw_negative_lmt) {
                axis_pv(axis, 0, axis->home_low_speed);
                axis->home_step = 3;
            }
            else if (axis->inputs.hw_slow_lmt == 0 && axis->inputs.home_high_speed_enable) {
                //回零减速信号位有信号，速度要采用低速回零
                axis_pv(axis, 1, axis->home_high_speed);
                axis->home_step = 2;
            }
            else {
                //以回零高速查找零点信号
                axis_pv(axis, 0, axis->home_low_speed);
                axis->home_step = 3;
            }
        }
        break;
    case 2:
        if (axis->inputs.hw_slow_lmt) {
            axis_pv_change_speed(axis, axis->home_low_speed);
            axis->home_step++;
        }
        break;
    case 3:
        if (axis->inputs.hw_home_sgn) {
            //查到了零点， 先减速停止
            axis_stop(axis);
            axis->home_step++;

        }
        break;
    case 4:
        if (axis->outputs.on == 0) {
            //离开原点一定距离
            axis_pp(axis, 0, axis->home_space, axis->home_low_speed);
            axis->home_step++;
        }
        break;
    case 5:
        if (axis->outputs.on == 0) {
            if (axis->inputs.hw_home_sgn) {
                axis->home_step = 4;
            }
            else {
                //以低速回退来查找原点
                axis_pv(axis, 0, axis->home_find_speed);
                axis->home_step++;
            }
        }
        break;
    case 6:
        if (axis->inputs.hw_home_sgn) {
            AXIS_OFF(axis);
            axis->home_step++;
        }
        break;
    case 7:
        if (axis->outputs.on == 0) {
            //查到零点，停止

            axis->home_step++;
        }
        break;
    case 8:
        if (axis->outputs.on == 0) {
            //再走一段设置距离
            axis_pp(axis, 0, axis->home_offset, axis->home_low_speed);
            axis->home_step++;
        }
        break;
    case 9:
        if (axis->outputs.on == 0) {
            //回零完毕
            axis->home_step = 0;
            axis->position = 0;
            axis->encoder = 0;
        }
        break;
    }
}

/**
 * 回零方式0
 * 
 * @author lxz (2019/6/10/周一)
 * 
 * @param axis 
 */
void axis_home_method0(axis_object_t *axis) {
    
        switch (axis->home_step)
        {
        case 1:
            //负限位或者零点有信号 ，需要向前运动
            if (axis->inputs.hw_home_sgn)
            {
                //先是离开一定的距离
                axis->home_step = 4;

            }
            else
            {
                if (axis->inputs.hw_negative_lmt)
                {
                    axis_pv(axis, 1, axis->home_low_speed);
                    axis->home_step = 3;
                    
                }
                else if (axis->inputs.hw_slow_lmt == 0 && axis->inputs.home_high_speed_enable)
                {
                    //回零减速信号位有信号，速度要采用低速回零
                    axis_pv(axis, 0, axis->home_find_speed);
                    axis->home_step = 2;

                }
                else
                {
                    //以回零高速查找零点信号
                    axis_pv(axis, 0, axis->home_low_speed);
                    axis->home_step = 3;
                        
                }
            }
            break;
        case 2:
          
            if (axis->inputs.hw_slow_lmt)
            {
                axis_pv_change_speed(axis, axis->home_low_speed);
                axis->home_step++;
                
                   
            }
            break;
        case 3:
            if (axis->inputs.hw_home_sgn)
            {
                //查到了零点， 先减速停止
                axis_stop(axis);
                axis->home_step++;
                
             
            }
            break;
        case 4:
      
            if (axis->outputs.on == 0)
            {
                //离开原点一定距离
                axis_pp(axis, 0, axis->home_space, axis->home_find_speed);
                axis->home_step++;
                
                
            }
            break;
        case 5:
            if (axis->outputs.on == 0)
            {
                
               // if (axis->inputs.hw_home_sgn)
                {
                    
                //    axis->home_step = 4;
                }
              //  else
                {
                    //以低速回退来查找原点
                    axis_pv(axis, 1, axis->home_find_speed);
                    axis->home_step = 8;
                    axis->home_sgn_count = 0;
                }
            }
            break;
       
            
        case 8:   
            if(!axis->inputs.hw_home_sgn) //(axis->outputs.on == 0) //
            {
                //查到零点，停止
                axis_stop(axis);
                axis->home_step = 9;  
            }
            break;
        case 9:

            if (axis->outputs.on == 0)
            {
                //再走一段设置距离
                axis_pp(axis, 0, axis->home_offset, axis->home_find_speed);
                axis->home_step = 10; 
                   
            }
            break;
        case 10:
         
            if (axis->outputs.on == 0 && axis->inputs.fin_sgn == 0)
            {
                //回零完毕
                axis->home_step = 0;
                axis->position = 0;
                axis->encoder = 0;
            }
            break;
        }
    
 /*  
    switch (axis->home_step) {
    case 1:
        //负限位或者零点有信号 ，需要向前运动
        if (axis->inputs.hw_home_sgn) {
            //先是离开一定的距离
            axis->home_step = 4;
        }
        else {
            if (axis->inputs.hw_negative_lmt) {
                axis_pv(axis, 1, axis->home_low_speed);
                axis->home_step = 3;
            }
            else if (axis->inputs.hw_slow_lmt == 0 && axis->inputs.home_high_speed_enable) {
                //回零减速信号位有信号，速度要采用低速回零
                axis_pv(axis, 0, axis->home_high_speed);
                axis->home_step = 2;
            }
            else {
                //以回零高速查找零点信号
                axis_pv(axis, 0, axis->home_low_speed);
                axis->home_step = 3;
            }
        }
        break;
    case 2:
        if (axis->inputs.hw_slow_lmt) {
            axis_pv_change_speed(axis, axis->home_low_speed);
            axis->home_step++;
        }
        break;
    case 3:
        if (axis->inputs.hw_home_sgn) {
            //查到了零点， 先减速停止
            axis_stop(axis);
            axis->home_step++;

        }
        break;
    case 4:
        if (axis->outputs.on == 0) {
            //离开原点一定距离
            axis_pp(axis, 0, axis->home_space, axis->home_low_speed);
            axis->home_step++;
        }
        break;
    case 5:
        if (axis->outputs.on == 0) {
            if (axis->inputs.hw_home_sgn) {
                axis->home_step = 4;
            }
            else {
                //以低速回退来查找原点
                axis_pv(axis, 0, axis->home_find_speed);
                axis->home_step++;
                axis->home_sgn_count = 0;
            }
        }
        break;
//  case 6:
//      if (axis->inputs.hw_home_sgn) {
//          AXIS_OFF(axis);
//          axis->home_step++;
//      }
//      break;
    case 8:
        if (axis->outputs.on == 0) {
            //查到零点，停止

            axis->home_step++;
        }
        break;
    case 9:
        if (axis->outputs.on == 0) {
            //再走一段设置距离
            axis_pp(axis, 0, axis->home_offset, axis->home_low_speed);
            axis->home_step++;
        }
        break;
    case 10:
        if (axis->outputs.on == 0 && axis->inputs.fin_sgn == 0) {
            //回零完毕
            axis->home_step = 0;
            axis->position = 0;
            axis->encoder = 0;
        }
        break;
    }
    */
}

/**
 * 回零方式2
 * 
 * @author LXZ (010820)
 * 
 * @param axis 
 */
void axis_home_method2(axis_object_t *axis) {
    switch (axis->home_step) {
    case 1:
        //不在零点时，不需要重复检测
        if (!axis->inputs.hw_home_sgn) {
            //先是离开一定的距离
            axis->home_step++;
        }
        axis->home_sgn_count = 0;
        axis->home_step++;
        axis_pv(axis, 1, axis->home_low_speed);
        break;
    case 2:
        if (!axis->inputs.hw_home_sgn) {
            axis->home_step++;
            axis->home_sgn_count = 0;
        }
        break;
    case 3:
        //if (axis->inputs.hw_home_sgn) {
        //    axis_stop(axis);
        //    axis->home_step++;
        //}
        break;
    case 5:
        if (axis->outputs.on == 0) {
            //离开原点一定距离
            axis_pp(axis, 0, axis->home_offset, axis->home_low_speed);
            axis->home_step++;
        }
        break;
    case 6:
        if (axis->outputs.on == 0) {
            axis->home_step = 0;
            axis->position = 0;
            axis->encoder = 0;
        }
    }
}



/**
 * 开始回零动作
 * 
 * @author LXZ (031420)
 * 
 * @param axis  	 轴对象
 * @param method     回零方式
 * @param home_speed 回零速度
 * @param find_speed 查找零点速度
 */
void axis_home(axis_object_t *axis, int method, int home_speed, int find_speed) {
    axis->home_method = method;
    axis->home_step = 1;
    axis->home_find_speed = find_speed;
    axis->home_low_speed = home_speed;
}

/**
 * 使能高速回原点的功能
 * 当回零系统中存在减速开关的时候，使能高速功能允许执行二段速回零，当没有减速信号时采用
 * 高速动作，检测到开关后会变为低速返回零点从而更加有效的加零。
 * 
 * @author LXZ (031420)
 * 
 * @param axis 
 * @param high_speed 
 */
void axis_home_enable_high_speed(axis_object_t *axis, int high_speed) {
    axis->inputs.home_high_speed_enable = 1;
    axis->home_high_speed = high_speed;
}


/**
 * 禁止高速回原点的功能
 * 
 * @author LXZ (031420)
 * 
 * @param axis 
 */
void axis_home_disable_high_speed(axis_object_t *axis) {
    axis->inputs.home_high_speed_enable = 0;
}

/**
 * 设置回零信号
 * 
 * @author LXZ (062820)
 * 
 * @param axis 轴对象
 * @param reg 管脚状态寄存器
 * @param mask 管脚位掩
 * @param value 管脚有效值
 * @param filter 滤波次数 
 */
void axis_set_home_sgn(axis_object_t *axis, void * reg, unsigned short mask,unsigned short value, char filter) {
    axis->home_sgn_register = (volatile unsigned short *) reg;
    axis->home_sgn_mask = mask;
    axis->home_sgn_filter = filter;
    axis->home_sgn_level = value;
}

/**
 * 设置编码器零点信号
 * 
 * @author LXZ (062820)
 * 
 * @param axis 轴对象
 * @param reg 管脚状态寄存器
 * @param mask 管脚位掩
 * @param value 管脚有效值
 * @param filter 滤波次数 
 */
void axis_set_home_encoder_sgn(axis_object_t *axis, void * reg, unsigned short mask,unsigned short value, char filter) {
    axis->inputs.home_encode_sgn_enable = 1;
    axis->home_encoder_sgn_register = (volatile unsigned short *) reg;
    axis->home_encoder_sgn_mask = mask;
    axis->home_encoder_sgn_filter = filter;
    axis->home_encoder_sgn_level = value;
}

/**
 * 执行回零动作
 * 
 * @author lxz (2019/6/10/周一)
 * 
 * @param axis 
 */
void axis_home_run(axis_object_t *axis) {
    switch (axis->home_method) {
    case 2:
        axis_home_method2(axis);
        break;
    case 1:
        axis_home_method1(axis);
        break;
    default:
        axis_home_method0(axis);
        break;
    }
}


/**
* 对轴的一些输入输出信号进行集合处理，包括限位保护等
 * 
 * @author lxz 
 * 
 * @param axis 
 */
void axis_run(axis_object_t *axis) {
    axis->outputs.error_alarm = axis->inputs.alarm_sgn;


    axis_home_run(axis);

    if (axis->outputs.error_alarm) {
        axis_emgstop(axis);
    }

    //软件限位保护
    if (axis->inputs.sw_lmt_enable) {
        if (axis->outputs.on) {
            //正软件限位保护
            if (axis->outputs.dir && axis->position >= axis->sw_positive_limit) {
                axis_emgstop(axis);
                axis->outputs.sw_positive_alarm = 1;
            }
            else {
                axis->outputs.sw_positive_alarm = 0;
            }
            //负软件限位保护
            if (axis->outputs.dir == 0 && axis->position <= axis->sw_negative_limit) {
                axis_emgstop(axis);
                axis->outputs.sw_negative_alarm = 1;
            }
            else {
                axis->outputs.sw_negative_alarm = 0;
            }
        }
    }
    else {
        axis->outputs.sw_negative_alarm = 0;
        axis->outputs.sw_positive_alarm = 0;
        axis->outputs.sw_negative_pre_alarm = 0;
        axis->outputs.sw_positive_pre_alarm = 0;
    }


    //正硬件限位保护
    if (axis->outputs.dir && axis->inputs.hw_positive_lmt) {
        axis_emgstop(axis);
        axis->outputs.hw_positive_alarm = 1;
    }
    else {
        axis->outputs.hw_positive_alarm = 0;
    }

    //负硬件限位保护
    if (axis->outputs.dir == 0 && axis->inputs.hw_negative_lmt) {
        axis_emgstop(axis);
        axis->outputs.hw_negative_alarm = 1;
    }
    else {
        axis->outputs.hw_negative_alarm = 0;
    }

    if (axis->outputs.enable) {
        AXIS_SET_EN(axis, 1);
    }
    else {
        AXIS_SET_EN(axis, 0);
    }

    //当回零步不为0的时候，说明正在回零，这时候一些操作就要注意不要冲突
    axis->outputs.homing = axis->home_step != 0;
    axis->outputs.on = AXIS_IS_ON(axis) != 0;

}


/**
 * 操作伺服对应IO
 * 
 * @author xuzhenglim:276137500 (2019-12-5)
 * 
 * @param axis 
 * @param type 
 * @param value 
 */
void axis_set_ouput(axis_object_t *axis, axis_output_name type, int value) {
    switch (type) {
    case AXIS_OUT_ENABLE:
        axis->outputs.enable = value;
        break;
    case AXIS_OUT_ERRCLEAR:
        axis->outputs.error_clear = value;
        break;

    }
}




/**
 * 获取报警值
 * 
 * @author LXZ (121919)
 * 
 * @param axis 	轴对象
 * 
 * @return int 0 没有报警 
 *                bit 0 伺服报警
 *                bit 1 硬件正限位
 *                bit 2 硬件负限位
 *                bit 3 软件正限位
 *                bit 4 软件负限位
 *                bit 5 软件负限位预报警
 *                bit 6 软件负限位预报警
 *  
 */
int axis_get_error(axis_object_t *axis) {
    int flag = 0;
    if (axis->outputs.error_alarm) {
        flag |= 1 << 0;
    }
    if (axis->outputs.hw_positive_alarm) {
        flag |= 1 << 1;
    }
    if (axis->outputs.hw_negative_alarm) {
        flag |= 1 << 2;
    }
    if (axis->outputs.sw_positive_alarm) {
        flag |= 1 << 3;
    }
    if (axis->outputs.sw_negative_alarm) {
        flag |= 1 << 4;
    }
    if (axis->outputs.sw_positive_pre_alarm) {
        flag |= 1 << 5;
    }
    if (axis->outputs.sw_negative_pre_alarm) {
        flag |= 1 << 6;
    }
//  if (axis->outputs.sw_feed_zero_alarm) {
//      flag |= 1 << 7;
//  }

    return flag;
}

/**
 * 清除报警
 * 
 * @author LXZ (052620)
 * 
 * @param axis 
 */
void axis_clear_error(axis_object_t *axis) {
    axis->outputs.sw_negative_alarm = 0;
    axis->outputs.sw_positive_alarm = 0;
    axis->outputs.sw_positive_pre_alarm = 0;
    axis->outputs.sw_negative_pre_alarm = 0;
    axis->outputs.sw_feed_zero_alarm = 0;
}

/**
 * 判断是否能正转
 * 
 * @author LXZ (010120)
 * 
 * @param axis 
 * 
 * @return int 
 */
int axis_cw_lmt(axis_object_t *axis) {
    return (axis->outputs.on == 0 &&
            axis->outputs.error_alarm == 0 &&
            axis->outputs.homing == 0 &&
            axis->outputs.hw_positive_alarm == 0 &&
            axis->outputs.sw_positive_alarm == 0);
}
/**
 * 判断是否能反转
 * 
 * @author LXZ (010120)
 * 
 * @param axis 
 * 
 * @return int 
 */
int axis_ccw_lmt(axis_object_t *axis) {
    return (axis->outputs.on == 0 &&
            axis->outputs.error_alarm == 0 &&
            axis->outputs.homing == 0 &&
            axis->outputs.hw_negative_alarm == 0 &&
            axis->outputs.sw_negative_alarm == 0);
}
/**
* 设置软件限位
* 
* @author LXZ (031420)
* 
* @param axis 		轴对象 
* @param positive  软件正限位 
* @param negative  软件负限位
*/
void axis_set_software_lmt(axis_object_t *axis, int positive, int negative) {
    if (axis->inputs.ratio_enable) {
        axis->sw_positive_limit = axis_position_to_pluse(axis, positive);
        axis->sw_negative_limit = axis_position_to_pluse(axis, negative);
    }
    else {
        axis->sw_positive_limit = positive;
        axis->sw_negative_limit = negative;
    }
}


/**
 * 使能软件限位
 * 
 * @author LXZ (031420)
 * 
 * @param axis 		轴对象 
 */
void axis_enable_software_lmt(axis_object_t *axis) {
    axis->inputs.sw_lmt_enable = 1;
}


/**
 * 关闭软件限位
 * 
 * @author LXZ (031420)
 * 
 * @param axis 		轴对象 
 * @param positive  软件正限位 
 * @param negative  软件负限位
 */
void axis_disable_software_lmt(axis_object_t *axis) {
    axis->inputs.sw_lmt_enable = 0;
}

/**
 * 设置加减速模式，包括时间
 * 
 * @author LXZ (031420)
 * 
 * @param axis 		轴对象
 * @param mode
 *  			 加减模式，0是直线加减速，1是S型加减速
 * @param acc_time  加速度时间
 * @param dec_time  减速度时间
 */
void axis_set_accdec(axis_object_t *axis, int mode, int acc_time, int dec_time) {
    axis->accdec_mode = mode;
    axis->acc_time = acc_time;
    axis->dec_time = dec_time;
}

/**
 * 使能调中回调函数
 * 
 * @author LXZ (031420)
 * 
 * @param axis   轴对象
 * @param handle 回调函数入口
 */
void axis_register_irq_callback(axis_object_t *axis, void (*handle)()) {
    axis->handle = handle;
}

/**
 * 禁止中断回调
 * 
 * @author LXZ (031420)
 * 
 * @param axis 
 */
void axis_unregister_irq_callback(axis_object_t *axis) {
    axis->handle = (void *)0;
}

/**
 * 设置电子齿轮比
 * 
 * @author LXZ (051820)
 * 
 * @param axis     
 * @param cycle_pluse   一圈脉冲数 
 * @param cycle_length  一圈长度
 * @param lenrate        放大倍数
 */
void axis_set_ratio(axis_object_t *axis, int cycle_pluse, int cycle_length,int lenrate)
{
    axis->cycle_length = cycle_length;
    axis->cycle_pluse = cycle_pluse;
    axis->length_rate = lenrate;
}


/**
 * 使能齿轮比定位
 * 
 * @author LXZ (060120)
 * 
 * @param axis 
 */
void axis_enable_ratio(axis_object_t *axis) {
    axis->inputs.ratio_enable = 1;
}

/**
 * 禁止齿轮比定位
 * 
 * @author LXZ (060120)
 * 
 * @param axis 
 */
void axis_disable_ratio(axis_object_t *axis) {
    axis->inputs.ratio_enable = 0;
}


/**
 * 位置转脉冲
 * 
 * @author LXZ (051820)
 * 
 * @param axis 
 */
int axis_position_to_pluse(axis_object_t *axis, int value)
{
    double length = (double)(axis->cycle_length) / axis->length_rate;
    unsigned int pluse = axis->cycle_pluse;
    double res = value;
    //参数保护
    if (length == 0)
    {
        length = 1;
    }
    if (pluse == 0)
    {
        pluse = 1;
    }
    res *= pluse;
    res /= length;
    if (res > 0)
    {
        res += 0.5;
    }
    else
    {
        res -= 0.5;
    }
    return (int)(res);
}


/**
 * 脉冲转位置
 * 
 * @author LXZ (051820)
 * 
 * @param axis 
 */
int axis_pluse_to_position(axis_object_t *axis, int value)
{
    double length = (double)(axis->cycle_length) / axis->length_rate;
    unsigned int pluse = axis->cycle_pluse;
    double res = value;
    //参数保护
    if (length == 0)
    {
        length = 1;
    }
    if (pluse == 0)
    {
        pluse = 1;
    }
    res *= length;
    res /= pluse;
    if (res > 0)
    {
        res += 0.5;
    }
    else
    {
        res -= 0.5;
    }
    return (int)(res);
}

/**
 * 转速转频率
 * 
 * @author LXZ (051820)
 * 
 * @param axis      轴对象
 * @param value     转速
 * 
 * @return int      转换结果（频率）
 */
int axis_revs_to_freq(axis_object_t *axis, int value) {
    unsigned int pluse = axis->cycle_pluse;
    int res = 0;
    if (pluse == 0) {
        pluse = 1;
    }
    res = pluse * value / 60;
    return res;
}

/**
 * 频率转转速
 * 
 * @author LXZ (051820)
 * 
 * @param axis      轴对象
 * @param value     频率
 * 
 * @return int      转换结果（转速）
 */
int axis_freq_to_revs(axis_object_t *axis, int value) {
    unsigned int pluse = axis->cycle_pluse;
    int res = 0;
    if (pluse == 0) {
        pluse = 1;
    }
    res = value * 60 / pluse;
    return res;
}

/**
 * @brief  速度单位 单位距离 / min ，返回频率
 *   距离单位为 0.01
 * @param axis_no
 * @param value
 * @return int
 */
int axis_speed_to_freq(axis_object_t *axis, int value)
{
    double length = (double)(axis->cycle_length) / axis->length_rate;
    unsigned int pluse = axis->cycle_pluse;
    double res;
    //参数保护
    if (length == 0)
    {
        length = 1;
    }
    if (pluse == 0)
    {
        pluse = 1;
    }

    res = pluse / length;
    res = res * value;
    res = res / 60;
    //ratio = ratio * value / 60;
    return (int)(res);
}