GC23005/Component/axis_motion/axis_motion.c

#include "global.h"
#include "axis_motion.h"

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

#define AXIS_ACC_REG_TABLE(axis)	axis->acc_reg_table
#define AXIS_ACC_NUMBER_TABLE(axis)	axis->acc_number_table

#ifndef AXIS_USING_ACC_TABLE_ONLY
#define AXIS_DEC_REG_TABLE(axis)	axis->dec_reg_table
#define AXIS_DEC_NUMBER_TABLE(axis)	axis->dec_number_table
#else
#define AXIS_DEC_REG_TABLE(axis)	axis->acc_reg_table
#define AXIS_DEC_NUMBER_TABLE(axis)	axis->acc_number_table
#endif

/**
* 轴使能
*
* @author LXZ (121919)
*
* @param axis
*/
void axis_enable(axis_object_t *axis) {
  axis->outputs.enable = 1;
  if (axis->inputs.en_reverse) {
    hw_pwm_set_enable(axis->axis_no,0);
  }
  else{
    hw_pwm_set_enable(axis->axis_no,1);
  }
}

/**
* 轴禁止
*
* @author LXZ (121919)
*
* @param axis
*/
void axis_disable(axis_object_t *axis) {
  axis->outputs.enable = 0;
  if (axis->inputs.en_reverse) {
    hw_pwm_set_enable(axis->axis_no,1);
  }
  else{
    hw_pwm_set_enable(axis->axis_no,0);
  }
}

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


#define AXIS_USING_FLOAT 
#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 ;
  int stop_speed = axis->stop_speed;
  float clock_ratio = axis->clock / 1000000;
  //周期临时值
  //int period;
  int reg_Value = 0;
  int last_reg_value = 0;
  
  float period;
  period=0.5;
  
  //period = axis->period;
  //限制加速度周期
  //if (period < 1) period = 2;
  
  //限制最小加速度时间单位
  acc_steps =(int)( axis->acc_time / period);
  if (acc_steps < 10) {
    acc_steps = 10;
  }
  
  dec_steps =(int)(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;
  //尽量保证最大加速度不会大于限制值
  if(cur_max_accdec>=0)
  {
    if (cur_max_accdec > axis->max_acc) {
      cur_max_accdec = axis->max_acc;
    }
  }
  else
  {
    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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      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_max_accdec>=0)&&(cur_speed < target_speed))
            ||((cur_max_accdec<0)&&(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->stop_speed) / dec_steps;
  
  //计算加速度会达到的最大值
  if (cur_max_accdec > axis->max_acc) {
    cur_max_accdec = axis->max_acc;
  }
  dec_steps = (int)((target_speed - axis->stop_speed) / cur_max_accdec);
  
  //开始减速表计算
  index = 0;
  axis->acc_position=0;
  last_reg_value = 0;
  
  cur_speed = stop_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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      if (AXIS_DEC_NUMBER_TABLE(axis)[index] < axis->min_steps) AXIS_DEC_NUMBER_TABLE(axis)[index] = axis->min_steps;
      axis->acc_position+=AXIS_ACC_NUMBER_TABLE(axis)[index];
#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;      //减速时间单位数
  long cur_accdec = 0;  //当前加减速度
  long cur_max_accdec = 0; //当前最大加速度减速度
  long cur_speed = 0;   //当前速度
  long ddc_speed = 0;   //减减速开始速度
  long cur_aacddc = 0;  //当前加加速减减速
  long mid_speed = 0;   //中间速度
  int index = 0;
  long clock = axis->clock;
  long clock_ratio = axis->clock / 1000000;
  long start_speed = axis->start_speed*10;
  long cur_target_speed=target_speed*10;
  long stop_speed=axis->stop_speed*10;
  //周期临时值
  //int period;
  long period;
  int reg_Value = 0;
  int last_reg_value;
  
  memset(&axis->acc_number_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  memset(&axis->acc_reg_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  memset(&axis->dec_number_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  memset(&axis->dec_reg_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  
  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;
  //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 = ((cur_target_speed - start_speed)) / (acc_steps * acc_steps);
  //保证至少1以上的加减速度
  if (cur_aacddc >= 0 && cur_aacddc < 1) {
    cur_aacddc = 1;
  }else if (cur_aacddc < 0 && cur_aacddc > -1) {
    cur_aacddc = -1;
  }
  //计算加速度会达到的最大值
  cur_max_accdec = cur_aacddc * acc_steps;
  if (cur_max_accdec > 0 && cur_max_accdec > axis->max_acc*10) {
    cur_max_accdec = axis->max_acc*10;
  }else if(cur_max_accdec < 0 && -cur_max_accdec > axis->max_acc*10) {
    cur_max_accdec = -axis->max_acc*10;
  }
  
  //限制加加速度，加加速度过大会导致过冲
  if (cur_aacddc > 0 && cur_aacddc > axis->max_aac*10) {
    cur_aacddc = axis->max_aac;
  }else if(cur_aacddc < 0 && -cur_aacddc > axis->max_aac*10) {
    cur_aacddc = -axis->max_aac*10;
  }  
  //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
  acc_steps = (int)(cur_max_accdec / cur_aacddc);
  if(acc_steps < 0) acc_steps = -acc_steps;
  
  //开始计算加速表
  mid_speed = (cur_target_speed + start_speed) / 2;
  ddc_speed = cur_target_speed - ((cur_aacddc * acc_steps * acc_steps) / 2);
  if (ddc_speed < mid_speed) {
    ddc_speed = mid_speed;
  }
  cur_speed = start_speed;
  axis->acc_position=0;
  
  //S加速曲线加速表计算
  do {
    //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
    //否则会造成波形中间下移，导致减加速流程过长
    if (cur_aacddc > 0 && cur_speed >= mid_speed)  {
      cur_accdec = cur_max_accdec;
      //保证只能进一次
      mid_speed *= 3;
    }else if(cur_aacddc < 0 && cur_speed <= mid_speed){
      cur_accdec = -cur_max_accdec;
      //保证只能进一次
      mid_speed = 0;
    }
    
    if (cur_aacddc > 0 && cur_speed >= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
      //减加速流程
      if (cur_accdec > cur_aacddc) {
        cur_accdec -= cur_aacddc;
      }
      else {
        //保证至少有这个加加速，让速度有机会达到目标速度
        cur_accdec = cur_aacddc;
      }
    }else if(cur_aacddc < 0 && cur_speed <= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
      //减加速流程
      if (-cur_accdec < cur_aacddc) {
        cur_accdec += cur_aacddc;
      }
      else {
        //保证至少有这个加加速，让速度有机会达到目标速度
        cur_accdec = -cur_aacddc;
      }
    }
    
    //计算速度
    if(cur_aacddc>0)
      cur_speed += cur_accdec;
    else
      cur_speed -= cur_accdec;
    if (cur_aacddc >= 0 && cur_speed > cur_target_speed) cur_speed = cur_target_speed;
    if (cur_aacddc < 0 && cur_speed < cur_target_speed) cur_speed = cur_target_speed;
    //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
    reg_Value = (int)((clock*10+5) / 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;
      }
      axis->acc_position+=AXIS_ACC_NUMBER_TABLE(axis)[index];
#ifdef AXIS_USING_DEBUG
      axis->acc_speed_table[index] = (int)cur_speed/10;
#endif
      index++;
#ifdef AXIS_IGNORE_REPEAT_STEP
    }
#endif
    if (cur_aacddc > 0 && cur_speed < ddc_speed) {        //还没到开始减速的速度
      //加加速流程
      cur_accdec += cur_aacddc;
      if (cur_accdec >= cur_max_accdec) {
        cur_accdec = cur_max_accdec;
      }
    }else if (cur_aacddc < 0 && cur_speed > ddc_speed) {        //还没到开始减速的速度
      //加加速流程
      cur_accdec -= cur_aacddc;
      if (-cur_accdec <= cur_max_accdec) {
        cur_accdec = -cur_max_accdec;
      }
    }
    
  } while (((cur_aacddc > 0 && cur_speed < cur_target_speed)
            ||(cur_aacddc < 0 && cur_speed > cur_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 = ((cur_target_speed - stop_speed)) / (dec_steps * dec_steps);
  
  //保证至少1以上的加减速度
  if (cur_aacddc >= 0 && cur_aacddc < 1) {
    cur_aacddc = 1;
  }else if (cur_aacddc < 0 && cur_aacddc > -1) {
    cur_aacddc = -1;
  }
  //计算加速度会达到的最大值
  cur_max_accdec = cur_aacddc * acc_steps;
  if (cur_max_accdec > 0 && cur_max_accdec > axis->max_acc*10) {
    cur_max_accdec = axis->max_acc*10;
  }else if(cur_max_accdec < 0 && -cur_max_accdec > axis->max_acc*10) {
    cur_max_accdec = -axis->max_acc*10;
  }
  //限制加加速度，加加速度过大会导致过冲
  if (cur_aacddc > 0 && cur_aacddc > axis->max_aac*10) {
    cur_aacddc = axis->max_aac*10;
  }else if(cur_aacddc < 0 && -cur_aacddc > axis->max_aac*10) {
    cur_aacddc = -axis->max_aac*10;
  }
  //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
  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 = (cur_target_speed + stop_speed) / 2;
  ddc_speed = cur_target_speed - ((cur_aacddc * dec_steps * dec_steps) / 2);
  
  if (ddc_speed < mid_speed) {
    ddc_speed = mid_speed;
  }
  cur_speed = stop_speed;
  
  //S加速曲线减速表计算
  do {
    //正常的情况下，速度达到目标速度一半后，加速度肯定达到最大值，如果没有达到，必须强制达到
    //否则会造成波形中间下移，导致减加速流程过长
    
    if (cur_aacddc > 0 && cur_speed >= mid_speed)  {
      cur_accdec = cur_max_accdec;
      //保证只能进一次
      mid_speed *= 3;
    }else if(cur_aacddc < 0 && cur_speed <= mid_speed){
      cur_accdec = -cur_max_accdec;
      //保证只能进一次
      mid_speed = 0;
    }
    if (cur_aacddc > 0 && cur_speed >= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
      //减加速流程
      if (cur_accdec > cur_aacddc) {
        cur_accdec -= cur_aacddc;
      }
      else {
        //保证至少有这个加加速，让速度有机会达到目标速度
        cur_accdec = cur_aacddc;
      }
    }else if(cur_aacddc < 0 && cur_speed <= ddc_speed) {  //减减速需要提前判断，尽量保证流程对称
      //减加速流程
      if (-cur_accdec < cur_aacddc) {
        cur_accdec += cur_aacddc;
      }
      else {
        //保证至少有这个加加速，让速度有机会达到目标速度
        cur_accdec = -cur_aacddc;
      }
    }        
     //计算速度
    if(cur_aacddc>0)
      cur_speed += cur_accdec;
    else
      cur_speed -= cur_accdec;
    if (cur_speed > cur_target_speed) cur_speed = cur_target_speed;
    //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
    reg_Value = (int)((clock*10+5) / 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_aacddc > 0 && cur_speed < ddc_speed) {        //还没到开始减速的速度
      //加加速流程
      cur_accdec += cur_aacddc;
      if (cur_accdec >= cur_max_accdec) {
        cur_accdec = cur_max_accdec;
      }
    }else if (cur_aacddc < 0 && cur_speed > ddc_speed) {        //还没到开始减速的速度
      //加加速流程
      cur_accdec -= cur_aacddc;
      if (-cur_accdec <= cur_max_accdec) {
        cur_accdec = -cur_max_accdec;
      }
    }
    
  } while (((cur_aacddc > 0 && cur_speed < cur_target_speed)
            ||(cur_aacddc < 0 && cur_speed > cur_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
}



/**
* 以两段直线加减速的方式计算加减速表
*      
* @author LXZ (021720)
*
* @param axis
* @param target_speed
*/
static void axis_mode2_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 ;
  int stop_speed = axis->stop_speed;
  float clock_ratio = axis->clock / 1000000;
  //周期临时值
  //int period;
  int reg_Value = 0;
  int last_reg_value = 0;
  
  float period;
  period=0.5;
  
  //两段直线加减速参数
  int acc_steps1,acc_steps2,dec_steps1,dec_steps2;
  int target_speed1,target_speed2;
  
  //period = axis->period;
  //限制加速度周期
  //if (period < 1) period = 2;
  
  //限制最小加速度时间单位
  acc_steps = (int)(axis->acc_time / period);
  if (acc_steps < 10) {
    acc_steps = 10;
  }
  dec_steps = (int)(axis->dec_time / period);
  if (dec_steps < 10) {
    dec_steps = 10;
  }
  //计算第一段直线加速的步数和目标速度
  if(target_speed >= axis->start_speed)
  {
    //第一段直线以1/10的时间加速到1/4的速度
    acc_steps1=acc_steps / 10;
    target_speed1=((target_speed-axis->start_speed) / 4)+axis->start_speed;
  }
  else
  {
    acc_steps1=acc_steps * 9 / 10;
    target_speed1=((target_speed-axis->start_speed) * 3 / 4)+axis->start_speed;
  }
  //计算第二段直线加速的步数和目标速度
  acc_steps2=acc_steps-acc_steps1;
  target_speed2=target_speed;
  
  //转换倍率
  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_speed1 - axis->start_speed) / acc_steps1;
  //尽量保证最大加速度不会大于限制值
  if(cur_max_accdec>=0)
  {
    if (cur_max_accdec > axis->max_acc) {
      cur_max_accdec = axis->max_acc;
    }
  }
  else
  {
    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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      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_max_accdec>=0)&&(cur_speed < target_speed1))
            ||((cur_max_accdec<0)&&(cur_speed > target_speed1)))
           && (index + 1) < AXIS_MAX_ACCDEC_STEP);
  
  //第二段加速直线
  //计算加速度会达到的最大值
  cur_max_accdec = (float)(target_speed2 - target_speed1) / acc_steps2;
  //尽量保证最大加速度不会大于限制值
  if(cur_max_accdec>=0)
  {
    if (cur_max_accdec > axis->max_acc) {
      cur_max_accdec = axis->max_acc;
    }
  }
  else
  {
    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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      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_max_accdec>=0)&&(cur_speed < target_speed2))
            ||((cur_max_accdec<0)&&(cur_speed > target_speed2)))
           && (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
  //开始减速表计算
  //计算第一第二段直线减速的步数
  dec_steps1=dec_steps / 10;
  dec_steps2=dec_steps-dec_steps1;
  if(axis->stop_speed > target_speed) axis->stop_speed = target_speed;
  target_speed1=((target_speed-axis->stop_speed) / 4)+axis->stop_speed;
  target_speed2=target_speed;
  
  //第一段减速直线
  cur_max_accdec = (target_speed1 - axis->stop_speed) / dec_steps1;
  
  //计算加速度会达到的最大值
  if (cur_max_accdec > axis->max_acc) {
    cur_max_accdec = axis->max_acc;
  }
  //dec_steps1 = (int)((target_speed1 - axis->stop_speed) / cur_max_accdec);
  
  //开始减速表计算
  index = 0;
  last_reg_value = 0;
  
  cur_speed = stop_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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      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_speed1) && (index + 1) < AXIS_MAX_ACCDEC_STEP);
  
  //第二段减速直线
  cur_max_accdec = (target_speed2 - target_speed1) / dec_steps2;
  
  //计算加速度会达到的最大值
  if (cur_max_accdec > axis->max_acc) {
    cur_max_accdec = axis->max_acc;
  }
  //dec_steps2 = (int)((target_speed1 - axis->stop_speed) / cur_max_accdec);
  
  //开始减速表计算
  //index = 0;
  //last_reg_value = 0;
  
  //cur_speed = stop_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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      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_speed2) && (index + 1) < AXIS_MAX_ACCDEC_STEP);
  //保存减速表大小
  axis->dec_table_size = index;
  //if (axis->dec_table_size > 0) {
  //  axis->dec_table_size -= 1;
  //}
#endif
}


/**
* 以固定斜率直线加减速的方式计算加减速表，加减速时间设置自动失效
*  
* @author LXZ (021720)
*
* @param axis
* @param target_speed
*/
static void axis_mode3_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 ;
  int stop_speed = axis->stop_speed;
  float clock_ratio = axis->clock / 1000000;
  //周期临时值
  //int period;
  int reg_Value = 0;
  int last_reg_value = 0;
  
  float period;
  period=0.5;
  
  //period = axis->period;
  //限制加速度周期
  //if (period < 1) period = 2;
  
  //限制最小加速度时间单位
  //acc_steps = axis->acc_time / period;
  
  //固定加减速斜率
  int accdec_value;
  if(target_speed > axis->start_speed) accdec_value=200;
  else accdec_value=-200;
  
  /*acc_steps = (target_speed - axis->start_speed) / accdec_value;
  if (acc_steps < 10) {
  acc_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;
  //尽量保证最大加速度不会大于限制值
  if(cur_max_accdec>=0)
  {
  if (cur_max_accdec > axis->max_acc) {
  cur_max_accdec = axis->max_acc;
}
}
        else
  {
  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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      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 += accdec_value;
    
  } while ((((accdec_value>=0)&&(cur_speed < target_speed))
            ||((accdec_value<0)&&(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->stop_speed) / dec_steps;
  /*
  //计算加速度会达到的最大值
  if (cur_max_accdec > axis->max_acc) {
  cur_max_accdec = axis->max_acc;
}
  dec_steps = (int)((target_speed - axis->stop_speed) / cur_max_accdec);
  */
  
  //开始减速表计算
  index = 0;
  last_reg_value = 0;
  
  //固定斜率
  accdec_value=200;
  
  if(target_speed < stop_speed) 
    stop_speed = target_speed;
  
  cur_speed = stop_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 + (reg_Value >> 1)) / reg_Value * clock_ratio);
      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 += accdec_value;
    
  } 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
}
/**
* 以指数曲线减速的方式计算加减速表
*
* @author LAZ (024608)
*
* @param axis 轴对象
* @param target_speed 目标速度
*/
static void axis_mode4_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; //当前最大加速度减速度
  long cur_speed = 0;   //当前速度
//  long ddc_speed = 0;   //减减速开始速度
  long cur_aacddc = 0;  //当前加加速减减速
  int index = 0;
  int i;
  long clock = axis->clock;
  long clock_ratio = axis->clock / 1000000;
  long start_speed = (long)(axis->start_speed*10);
  long cur_target_speed=(long)(target_speed*10);
  long stop_speed;
  //周期临时值
  long period;
  int reg_Value = 0;
  
  memset(&axis->acc_number_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  memset(&axis->acc_reg_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  memset(&axis->dec_number_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  memset(&axis->dec_reg_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  
  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;
  cur_aacddc = abs(cur_target_speed - start_speed);
  cur_aacddc =cur_aacddc*2/(acc_steps * acc_steps);  
  //速度放大10倍，保证至少0.1以上的加减速度，这样加速时间可延时到300ms
  if (cur_aacddc < 1)cur_aacddc = 1; 
  //计算加速度会达到的最大值
  cur_max_accdec = cur_aacddc * acc_steps;
  if (cur_max_accdec > axis->max_acc*10)cur_max_accdec = axis->max_acc*10; 
  //限制加加速度，加加速度过大会导致过冲
  if (cur_aacddc > axis->max_aac*10)cur_aacddc = axis->max_aac*10; 
  //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
  acc_steps = cur_max_accdec / cur_aacddc;  
  //目标速度大于起始速度，加速
  if(target_speed>=axis->start_speed)
  {
    cur_speed = (long)(axis->start_speed*10);
    cur_target_speed=(long)(target_speed*10);
    axis->acc_position=0;
    cur_accdec=0;
    index=0;
  }
  else//减速
  {
    cur_speed=target_speed*10;
    cur_target_speed=axis->start_speed*10;
    axis->acc_position=0;
    cur_accdec=0;
    index=0;
  }
  //指数加速曲线加速表计算
  do{      
    cur_speed += cur_accdec;//计算速度
    if (cur_speed > cur_target_speed) cur_speed = cur_target_speed;
    //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
    reg_Value = (clock*10+5) / cur_speed;
    if(target_speed>=axis->start_speed){ //加速    
      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;
      }
      axis->acc_position+=AXIS_ACC_NUMBER_TABLE(axis)[index];
#ifdef AXIS_USING_DEBUG
      axis->acc_speed_table[index] = (int)cur_speed/10;
#endif
    }
    else{  //减速，    
      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/10;
#endif
    }
    index++;
    if (cur_speed < cur_target_speed) { //还没到开始减速的速度
      //加加速流程
      cur_accdec += cur_aacddc;
      if (cur_accdec >= cur_max_accdec) {
        cur_accdec = cur_max_accdec;
      }
    }
  } while ((cur_aacddc > 0 && cur_speed < cur_target_speed)            
           && (index + 1) < AXIS_MAX_ACCDEC_STEP); 
  if(target_speed<axis->start_speed){//减速，调换减速顺序。
    for(i=0;i<index;i++)
    {
      AXIS_ACC_REG_TABLE(axis)[i] = AXIS_DEC_REG_TABLE(axis)[index-1-i];
      AXIS_ACC_NUMBER_TABLE(axis)[i] = AXIS_DEC_NUMBER_TABLE(axis)[index-1-i];
#ifdef AXIS_USING_DEBUG
      axis->acc_speed_table[i] = axis->dec_speed_table[index-1-i] ;
#endif
    }
  }
  //保存表大小
  axis->acc_table_size = index;
#ifdef AXIS_USING_ACC_TABLE_ONLY
  if (index > 0) {
    index -= 1;
  }
  axis->dec_table_size = index;
#else
  memset(&axis->dec_number_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  memset(&axis->dec_reg_table[0],0,2*AXIS_MAX_ACCDEC_STEP);
  //if (dec_steps != acc_steps) {
  stop_speed=axis->stop_speed*10;
  cur_target_speed=target_speed*10;
  if(stop_speed>cur_target_speed)stop_speed=cur_target_speed;
  cur_aacddc = ((cur_target_speed - stop_speed))*2 / (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*10)cur_max_accdec = axis->max_acc*10;
  //限制加加速度，加加速度过大会导致过冲
  if ( cur_aacddc > axis->max_aac*10) {
    cur_aacddc = axis->max_aac*10;
  }
  //由于加加速比计算结果小，所以需要更多步去达到最高速，需要重新加速步
  dec_steps = cur_max_accdec / cur_aacddc;
  //开始减速表计算
  index = 0;
  cur_accdec=0;
  cur_speed = stop_speed;
  //指数加速曲线减速表计算
  do {
    //计算速度
    cur_speed += cur_accdec;
    if (cur_speed > cur_target_speed) cur_speed = cur_target_speed;
    //计算该速度对应的寄存器值与以及当前频率下需要满足加速周期的脉冲速
    reg_Value = ((clock*10+5) / cur_speed);    
    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/10;
#endif
    index++;
    if (cur_speed < cur_target_speed) {        //还没到开始减速的速度
      //加加速流程
      cur_accdec += cur_aacddc;
      if (cur_accdec >= cur_max_accdec) {
        cur_accdec = cur_max_accdec;
      }
    }    
  } while (( cur_speed < cur_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;
  //尽量保证最大加速度不会大于限制值
  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 + (reg_Value >> 1)) / reg_Value) * clock_ratio;
      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;
  
  //计算加速度会达到的最大值
  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 + (reg_Value >> 1)) / reg_Value) * clock_ratio;
      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 = 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);
  
  //计算加速度会达到的最大值
  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 + (reg_Value >> 1)) / reg_Value) * clock_ratio;
      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);
    
    //计算加速度会达到的最大值
    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 + (reg_Value >> 1)) / reg_Value) * clock_ratio;
      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
}
#endif
/**
* 计算加减速表的接口
*
* @author LXZ (021720)
*
* @param axis
* @param target_speed
*/
static void axis_calc_speed_table(axis_object_t *axis, int target_speed) {
  switch(axis->accdec_mode)
  {
  case 0:
    axis_mode0_calc_accdec(axis, target_speed);
    break;
  case 1:
    axis_mode1_calc_accdec(axis, target_speed);
    break;
  case 2:
    axis_mode2_calc_accdec(axis, target_speed);
    break;
  case 3:
    axis_mode3_calc_accdec(axis, target_speed);
    break;
  case 4:
    axis_mode4_calc_accdec(axis, target_speed);
    break;
  default:
    axis_mode0_calc_accdec(axis, target_speed);
    break;
  }
}
/*
*脉冲细分分割函数，
*希望是每个脉冲产生后，改变一次脉冲频率，尽量使脉冲平滑过渡
*/
void axis_acc_pwm_divide(axis_object_t *axis)
{
  unsigned short reg_value = 0;
  if(axis->cur_speed_index+1 <=axis->acc_speed_index){//加速索引小于总的索引
    if(axis->cur_reg_value>AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index+1]){//加速时
      reg_value=axis->cur_reg_value-AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index+1];
      if(axis->accdec_count>reg_value){//如果脉冲数大于，频率的差值
        reg_value=1;              
      }
      else{//如果脉冲数小于频率的差值
        reg_value>>=1;
      }
      axis->cur_reg_value-=reg_value;
    }
    else if(axis->cur_reg_value<AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index+1]){//减速时
      reg_value=AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index+1]-axis->cur_reg_value;
      if(axis->accdec_count>reg_value){//如果脉冲数大于，频率的差值
        reg_value=1;              
      }
      else{//如果脉冲数小于频率的差值
        reg_value>>=1;
      }
      axis->cur_reg_value+=reg_value;
    }        
  }
}
void axis_dec_pwm_divide(axis_object_t *axis)
{
  unsigned short reg_value = 0;
  if(axis->cur_speed_index-1 >=0){//加速索引小于总的索引
    if(axis->cur_reg_value>AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index-1]){//加速时
      reg_value=axis->cur_reg_value-AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index-1];
      if(axis->accdec_count>reg_value){//如果脉冲数大于，频率的差值
        reg_value=1;              
      }
      else{//如果脉冲数小于频率的差值
        reg_value>>=1;
      }
      axis->cur_reg_value-=reg_value;
    }
    else if(axis->cur_reg_value<AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index-1]){//减速时
      reg_value=AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index-1]-axis->cur_reg_value;
      if(axis->accdec_count>reg_value){//如果脉冲数大于，频率的差值
        reg_value=1;              
      }
      else{//如果脉冲数小于频率的差值
        reg_value>>=1;
      }
      axis->cur_reg_value+=reg_value;
    }        
  }
}
/**
* 轴运动中断函数
*
* @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];
        axis->cur_reg_value = AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index];
      }
      else if (axis->accdec_count >1) { //减速脉冲计数
        axis->accdec_count--;
        axis_dec_pwm_divide(axis);
      } 
      else if (axis->cur_speed_index > 0) { //切换速度
        axis->cur_speed_index--;
        axis->accdec_count = AXIS_DEC_NUMBER_TABLE(axis)[axis->cur_speed_index];
        axis->cur_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--;
        axis_acc_pwm_divide(axis);
      } 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];
        axis->cur_reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
      }      
    }
    //到达位置立刻停止
    if ((axis->position - axis->target_position) * axis->feed >= 0) {
      if((axis->PP_Stop_Select) == 0)
        AXIS_OFF(axis);
    }
    //更新寄存器值
    AXIS_SET_PERION(axis, axis->cur_reg_value);    
    break;
#ifdef AXIS_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 != 2) {
          axis->cur_accdec_status = 2;
          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 != 2) {
          axis->cur_accdec_status = 2;
          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) {
      if(axis->cur_task_index < axis->task_number) axis->cur_task_index++;
      axis->cur_accdec_status = 0;
      if (axis->cur_task_index >= axis->task_number) {
        if((axis->PP_Stop_Select) == 0)
          AXIS_OFF(axis);
      }
    }
    //更新寄存器值
    AXIS_SET_PERION(axis, reg_value);
    axis->cur_speed=axis->clock/reg_value;
    
    break;
#endif
    
  case AXIS_MODE_PV:
    axis->cur_accdec_status = AXIS_ACCDEC_STATUS_ACC;
    if (axis->accdec_count > 1) {
      axis->accdec_count--;
      axis_acc_pwm_divide(axis);
    } 
    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];
      axis->cur_reg_value = AXIS_ACC_REG_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];
      axis->cur_reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
    } 
    else {
      axis->accdec_count = 0;
    }
    if (axis->accdec_count == 0 && axis->cur_speed_index == axis->acc_speed_index) {
      axis->cur_reg_value = axis->target_speed_reg;
    } 
    //更新寄存器值
    AXIS_SET_PERION(axis,axis->cur_reg_value);
    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];
    }
    axis->cur_reg_value = AXIS_DEC_REG_TABLE(axis)[axis->cur_speed_index];
    //更新寄存器值
    AXIS_SET_PERION(axis, axis->cur_reg_value);
    break;
  case AXIS_MODE_EMGSTOP:
    axis->home_step = 0;
    AXIS_OFF(axis);
    axis->cur_speed=0;
    break;
  case AXIS_MODE_HOLD:
    break;
  }
  
  if (axis->handle != 0) {
    axis->handle();
  }
  
}


/**
* 以目标速度移动到指定位置
*
* @author LXZ (021720)
*
* @param axis 轴对象
* @param rel
*  		  位置参考系，0表示相对坐标，1表进绝对值坐标
* @param position 目标位置
* @param target_speed 目标速度
*/
void axis_pp(axis_object_t *axis, int rel, int position, int target_speed,long slowpos) {
  //允许加速表可执行到的位置
  int acc_index = 0;
  //当前减速表可执行到的位置
  int dec_index = 0;
  //开始减速的位置
  int dec_position = 0;
  axis_calc_speed_table(axis, target_speed * axis->speed_unit);
  
  //计算目标停止位置
  axis->target_position = position;
  if (rel == 0) {
    axis->target_position += axis->position;
  } else {
    position -= axis->position;
    SetStopSelect(X_AXIS,PP_Stop); //绝对位置时要停止
  }
  
  //计算进给方向
  if (axis->target_position > axis->position) {
    axis_cw(axis);
  } else {
    axis_ccw(axis);
  }
  
  //换算成绝对值
  if (position < 0) {
    position = -position;
  }
  if(position>slowpos){
    position -= slowpos;
  }
  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]) 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) -(slowpos* axis->feed);
  
  //让中断切换到定位模式
  axis->run_mode = AXIS_MODE_PP;
  axis_start(axis);
}


/**
* pp模式中途改变速度
*
* @author LXZ (021720)
*
* @param axis 轴对象
* @param target_speed 目标速度
* @param slowpos 最后低速运行距离
*/
void axis_pp_change_speed (axis_object_t *axis, int rel, 
                           int position, int target_speed,long slowpos) {
                             //允许加速表可执行到的位置
                             int acc_index = 0;
                             //当前减速表可执行到的位置
                             int dec_index = 0;
                             //开始减速的位置
                             int dec_position = 0;
                             //开始重新计算参数前，先进入轴保持模式
                             axis->run_mode = AXIS_MODE_HOLD;
                             
                             //位置      
                             axis->target_position = position;
                             if (rel == 0) {
                               axis->target_position += axis->position;
                             } else {
                               position -= axis->position;
                               SetStopSelect(X_AXIS,PP_Stop); //绝对位置时要停止
                             }
                             //计算加减速表
                             axis_calc_speed_table(axis, target_speed * axis->speed_unit);
                             
                             //换算成绝对值
                             if (position < 0) {
                               position = -position;
                             }
                             //减掉最后低速运行距离
                             if(position>slowpos){
                               position -= slowpos;
                             }
                             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]) 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) -(slowpos* axis->feed);
                             axis->cur_speed_index = 0;
                             axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
                             axis->cur_accdec_status = 0;
                             axis->cur_reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
                             AXIS_SET_PERION(axis, AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index]);
                             //计算完成，切换回PP模式
                             axis->run_mode = AXIS_MODE_PP;
                           }


/**
* 以目标速度执行连续运动
*
* @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);
  axis_calc_speed_table(axis, target_speed * axis->speed_unit);
  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 (021720)
*
* @param axis 轴对象
* @param target_speed 目标速度
*/
void axis_pv_change_speed_table(axis_object_t *axis, int target_speed) {
  
  //开始计算前，先切换到保持模式
  axis->run_mode = AXIS_MODE_HOLD;
  //计算加减速表
  axis_calc_speed_table(axis, target_speed * axis->speed_unit);
  axis->acc_speed_index = axis->acc_table_size - 1;
  axis->target_speed_reg = AXIS_ACC_REG_TABLE(axis)[axis->acc_speed_index];
  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->run_mode = AXIS_MODE_PV;
}

/**
* 直接变速不经过加减速过程，支持PP、PV模式调用，调用后切换到连续运动PV模式
*
* @author LXZ (021720)
*
* @param axis 轴对象
* @param target_speed 目标速度
*/
void axis_change_speed_direct(axis_object_t *axis, int target_speed) {
  unsigned short reg_value = 0;
  int speed=target_speed * axis->speed_unit;
  //开始计算前，先切换到保持模式
  axis->run_mode = AXIS_MODE_HOLD;
  //直接更新寄存器值
  reg_value = axis->clock / speed;
  axis->cur_reg_value=reg_value;
  AXIS_SET_PERION(axis, reg_value);
  axis->cur_speed=speed;
  axis->start_speed = axis->cur_speed;
  //计算加减速表
  axis_calc_speed_table(axis, target_speed * axis->speed_unit);
  axis->acc_speed_index = axis->acc_table_size - 1;
  axis->target_speed_reg = AXIS_ACC_REG_TABLE(axis)[axis->acc_speed_index];
  axis->cur_speed_index = 0;
  axis->accdec_count = AXIS_ACC_NUMBER_TABLE(axis)[axis->cur_speed_index];
  axis->cur_accdec_status = 0;
  axis->cur_reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
  AXIS_SET_PERION(axis, AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index]);
  //让中断切换到连续运动模式
  axis->run_mode = AXIS_MODE_PV;        
}


#ifdef AXIS_TASK_NUMBER
/**
* 初始化轴任务缓冲，并设置此时的方向
*
* @author LXZ (031420)
*
* @param axis 轴对象
* @param dir  期望运动的方向
*/
void axis_init_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_TASK_NUMBER && feed > 0) {
    axis->tasks[axis->task_number].position = feed;
    axis->tasks[axis->task_number].speed = speed;
    axis->task_number++;
  }
}
/**
* 启动多段速定位运动模式
*
* @author LXZ (031420)
*
* @param axis
*/
void 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;
  
  //计算最高速度
  for (i = 0, speed = 0; i < axis->task_number; i++) {
    if  (speed < axis->tasks[i].speed) speed = axis->tasks[i].speed;
    
  }
  speed *= axis->speed_unit;
  
  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++) {
    
    speed = axis->tasks[i].speed * axis->speed_unit;
    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[dst_speed_index] > axis->tasks[i].reg_value) {
        dst_speed_index++;
      } else if (axis->acc_reg_table[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;
  }
  
  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);
}
#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;
    }
    else
    {  
      //到位不停模式下，直接停
      if(axis->PP_Stop_Select != 0)      
        axis->run_mode = AXIS_MODE_EMGSTOP;
    }
  }
}



/**
* 非停止的运行模式下保持当前方向运行到指定位置停止
* 支持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->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 (axis->cur_accdec_status == 2) {   //正在减速状态下，先查找相匹配的加速点
    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]) //剩余脉冲不足
            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 == 2) {
    if (dec_index < axis->cur_speed_index) {
      //由于当前要求的减速开始速度比当前速度要小，需要触发速度跳变
      axis->cur_accdec_status = 1;
    }
  }
  //切换运行模式
  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;
  speed *= axis->speed_unit;
  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->outputs.on = 0;
  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->cur_reg_value = AXIS_ACC_REG_TABLE(axis)[axis->cur_speed_index];
  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, axis_paramter_t 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;
    break;
  case AXIS_PERIOD:
    axis->period = argv;
    break;
  case AXIS_MIN_STEP:
    axis->min_steps = argv;
    break;
  case AXIS_SPEED_UNIT:
    axis->speed_unit = argv;
    break;
  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_START_SPEED:
    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_PP_STOP_SELECT:
    axis->PP_Stop_Select = argv;
    break;
  case AXIS_DIR_REVERSE:
    axis->inputs.dir_reverse = argv;
    break;
  case AXIS_EN_REVERSE:
    axis->inputs.en_reverse = argv;
    break;
  case AXIS_ALARM_REVERSE:
    axis->inputs.alarm_reverse = argv;
    break;
  case AXIS_USE_ENCODE:
      axis->use_encode = argv;
      break;
  }
}


/**
* 设置轴的参数
*
* @author LXZ (031420)
*
* @param axis 轴对象
* @param cmd  设置的对象
* @param argv 设置的参数
*
*/
int axis_get_parameter(axis_object_t *axis, axis_paramter_t 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;
  case AXIS_SPEED_UNIT:
    argv = axis->speed_unit;
    break;
  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_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;
  }
  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,0);
      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,0);
      axis->home_step++;
    }
    break;
  case 9:
    if (axis->outputs.on == 0) {
      //回零完毕
      axis->home_step = 0;
      axis->position = 0;
      axis->encoder = 0;
    }
    break;
  }
}

/**
* 回零方式1
*
* @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_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,0);
      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,0);
      axis->home_step++;
    }
    break;
  case 9:
    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_step++;
    axis_pv(axis, 1, axis->home_low_speed);
    break;
  case 2:
    if (!axis->inputs.hw_home_sgn) {
      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,0);
      axis->home_step++;
    }
    break;
  case 5:
    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 (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 && 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;
    }
  }
  
  
  //正硬件限位保护
  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 没有报警
*                 1 伺服报警
*                 2 软件正限位
*                 3 软件负限位
*                 4 硬件正限位
*                 5 硬件负限位
*/
axis_alarm_code_t axis_alarm_code(axis_object_t *axis) {
  if (axis->outputs.error_alarm) {
    return AXIS_ALARM_ERROR;
  } else if (axis->outputs.sw_positive_alarm) {
    return AXIS_ALARM_SW_POSITIVE;
  } else if (axis->outputs.sw_negative_alarm) {
    return AXIS_ALARM_SW_NEGATIVE;
  } else if (axis->outputs.hw_positive_alarm) {
    return AXIS_ALARM_HW_POSITIVE;
  } else if (axis->outputs.hw_negative_alarm) {
    return AXIS_ALARM_HW_NEGATIVE;
  } else return AXIS_ALARM_NONE;
}

/**
* 判断是否能正转
*
* @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.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.hw_negative_alarm == 0 &&
            axis->outputs.sw_negative_alarm == 0);
}

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


/**
* 关闭软件限位
*
* @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_enable_irq_callback(axis_object_t *axis, void (*handle)()) {
  axis->handle = handle;
}

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