GC23005/Component/axis_motion/AllSeroDrv.c

#include "global.h"
#include "AllServoDrv.h"
axis_object_t *axis_x;
axis_object_t *axis_y;
axis_object_t *axis_z;

float XGearRatio = 1,YGearRatio = 1,ZGearRatio = 1;

static int axis_x_ctrl_mode=0,axis_y_ctrl_mode=0,axis_z_ctrl_mode=0;
unsigned char axis_x_com_alarm=0,axis_y_com_alarm=0,axis_z_com_alarm=0;
void SetDir(unsigned short axis, unsigned short dir)
{
     switch(axis)
    {
        case X_AXIS: 
          if(dir)
            axis_cw(axis_x);
          else
            axis_ccw(axis_x);  
          break;
        case Y_AXIS: 
          if(dir)
            axis_cw(axis_y);
          else
            axis_ccw(axis_y);  
          break;
        case Z_AXIS: 
          if(dir)
            axis_cw(axis_z);
          else
            axis_ccw(axis_z);  
          break;
        default:;
    }
}

void SetDirReverse(unsigned short axis, int value)
{
     switch(axis)
    {
        case X_AXIS: 
            if(value)
              axis_set_parameter (axis_x,AXIS_DIR_REVERSE,1);
            else
              axis_set_parameter (axis_x,AXIS_DIR_REVERSE,0);
          break;
        case Y_AXIS: 
            if(value)
              axis_set_parameter (axis_y,AXIS_DIR_REVERSE,1);
            else
              axis_set_parameter (axis_y,AXIS_DIR_REVERSE,0);
          break;
        case Z_AXIS: 
            if(value)
              axis_set_parameter (axis_z,AXIS_DIR_REVERSE,1);
            else
              axis_set_parameter (axis_z,AXIS_DIR_REVERSE,0);
          break;
        default:;
    }
}



unsigned char GetDir(unsigned short axis)
{
     switch(axis)
    {
        case X_AXIS: return axis_x->outputs.dir;break;
        case Y_AXIS: return axis_y->outputs.dir;break;
        case Z_AXIS: return axis_z->outputs.dir;break;
        default:return axis_x->outputs.dir;
    }
}
//设置使能
void SetEn(unsigned short axis, unsigned short ena)
{
    switch(axis)
    {
        case X_AXIS:
          if(ena)
            axis_enable(axis_x);
          else
            axis_disable(axis_x);
          break;
        case Y_AXIS:
          if(ena)
            axis_enable(axis_y);
          else
            axis_disable(axis_y);
          break;
        case Z_AXIS:
          if(ena)
            axis_enable(axis_z);
          else
            axis_disable(axis_z);
          break;
        default:;
    }
}
//设置使能电平
void SetEnReverse(unsigned short axis, int value)
{
     switch(axis)
    {
        case X_AXIS: 
            if(value)
              axis_set_parameter (axis_x,AXIS_EN_REVERSE,1);
            else
              axis_set_parameter (axis_x,AXIS_EN_REVERSE,0);
          break;
        case Y_AXIS: 
            if(value)
              axis_set_parameter (axis_y,AXIS_EN_REVERSE,1);
            else
              axis_set_parameter (axis_y,AXIS_EN_REVERSE,0);
          break;
        case Z_AXIS: 
            if(value)
              axis_set_parameter (axis_z,AXIS_EN_REVERSE,1);
            else
              axis_set_parameter (axis_z,AXIS_EN_REVERSE,0);
          break;
        default:;
    }
}
//设置伺服告警的极性
void SetAlarmReverse(unsigned short axis, int value)
{
     switch(axis)
    {
        case X_AXIS: 
          if(value){
              axis_set_parameter (axis_x,AXIS_ALARM_REVERSE,1);
              set_com_alarm_reverse(axis,1);
          }
          else{
              axis_set_parameter (axis_x,AXIS_ALARM_REVERSE,0);
              set_com_alarm_reverse(axis,0);
          }
          break;
        case Y_AXIS: 
          if(value){
              axis_set_parameter (axis_y,AXIS_ALARM_REVERSE,1);
              set_com_alarm_reverse(axis,1);
          }
          else{
              axis_set_parameter (axis_y,AXIS_ALARM_REVERSE,0);
              set_com_alarm_reverse(axis,0);
          }
          break;
        case Z_AXIS: 
          if(value){
              axis_set_parameter (axis_z,AXIS_ALARM_REVERSE,1);
              set_com_alarm_reverse(axis,0);
          }
          else{
              axis_set_parameter (axis_z,AXIS_ALARM_REVERSE,0);
              set_com_alarm_reverse(axis,0);
          }
          break;
        default:;
    }
}
unsigned char GetEn(unsigned short axis)
{
    switch(axis)
    {
        case X_AXIS:return (axis_x->outputs.enable);break;
        case Y_AXIS:return (axis_y->outputs.enable);break;
        case Z_AXIS:return (axis_z->outputs.enable);break;
        default:return (axis_x->outputs.enable);
    }
    
}

unsigned char GetAlarm(unsigned short axis)
{
     switch(axis)
    {
        case X_AXIS:
          if(axis_x_ctrl_mode){//通讯模式                 
            return axis_x_com_alarm;
          }
          else {//IO模式   
            if(axis_x->inputs.alarm_reverse){
              if(hw_pwm_get_alr(axis_x->axis_no))return 1;else return 0;            
            }
            else 
              if(hw_pwm_get_alr(axis_x->axis_no))return 0;else return 1; 
          }
          break;
        case Y_AXIS:
          if(axis_y_ctrl_mode){//通讯模式
            return axis_y_com_alarm;
          }
          else {//IO模式   
            if(axis_y->inputs.alarm_reverse){
              if(hw_pwm_get_alr(axis_y->axis_no))return 1;else return 0;            
            }
            else 
              if(hw_pwm_get_alr(axis_y->axis_no))return 0;else return 1; 
          }
          break;
        case Z_AXIS:
          if(axis_z_ctrl_mode){//通讯模式          
            return axis_z_com_alarm;
          }
          else {//IO模式   
            if(axis_z->inputs.alarm_reverse){
              if(hw_pwm_get_alr(axis_z->axis_no))return 1;else return 0;            
            }
            else 
              if(hw_pwm_get_alr(axis_z->axis_no))return 0;else return 1;
          }
          break;
          default:return hw_pwm_get_alr(axis_x->axis_no);
    }
}
void SetClr(unsigned short axis, unsigned short clr)
{
    switch(axis)
    {
        case X_AXIS:
          if(axis_x_ctrl_mode){//通讯模式
            if(clr)clr_com_servo_alarm(axis);            
          }
          else{//IO模式
            hw_pwm_set_clr(axis_x->axis_no, clr);
          }
          break;
        case Y_AXIS:
          if(axis_y_ctrl_mode){//通讯模式
            if(clr)clr_com_servo_alarm(axis); 
          }
          else{//IO模式
            hw_pwm_set_clr(axis_y->axis_no, clr);
          }
          break;
        case Z_AXIS:
         if(axis_z_ctrl_mode){//通讯模式
            if(clr)clr_com_servo_alarm(axis); 
          }
          else{//IO模式
            hw_pwm_set_clr(axis_z->axis_no, clr);
          }
          break;
        default:
          break;
    }
}

unsigned char GetTarr(unsigned short axis)
{
     switch(axis)
    {
        case X_AXIS:return hw_pwm_get_tarr(axis_x->axis_no);break;
        case Y_AXIS:return hw_pwm_get_tarr(axis_y->axis_no);break;
        case Z_AXIS:return hw_pwm_get_tarr(axis_z->axis_no);break;
        default:return hw_pwm_get_tarr(axis_x->axis_no);
    }
}
/**
 * 切换控制模式切换，转矩+位置模式
 *
 * @author LAZ (121919)
 *
 * @param axis
 * @param mode，0为IO转矩模式，1为位置模式
 */
void Set_Ctrlmode_trans(unsigned short axis, unsigned short mode)
{
      switch(axis)
    {
        case X_AXIS:
            hw_pwm_set_tmode(axis_x->axis_no, mode);
          break;
        case Y_AXIS:
            hw_pwm_set_tmode(axis_y->axis_no, mode);
          break;
        case Z_AXIS:
            hw_pwm_set_tmode(axis_z->axis_no, mode);
          break;
        default:
          break;
    }
}
/**
 * 设置伺服的运行模式
 *
 * @author LAZ (121919)
 *
 * @param axis
 * @param mode，0为IO模式，1为通讯模式
 */
void Set_Servo_Runmode(unsigned short axis, unsigned short mode)
{
    switch(axis)
    {
        case X_AXIS:
            axis_x_ctrl_mode=mode;
          break;
        case Y_AXIS:
            axis_y_ctrl_mode=mode;
          break;
        case Z_AXIS:
            axis_z_ctrl_mode=mode;
          break;
        default:
          break;
    }
}

//脉冲转化为距离
long PulseToPos(unsigned short axis, long pulse)
{
    float pulse_buff;

    pulse_buff = pulse;
    switch(axis)
    {
        case X_AXIS: return (long)(pulse_buff/XGearRatio);break;
        case Y_AXIS: return (long)(pulse_buff/YGearRatio);break;
        case Z_AXIS: return (long)(pulse_buff/ZGearRatio);break;
        default:return (long)(pulse_buff/XGearRatio);;
    }
}

//距离转化为脉冲
long PosToPulse(unsigned short axis, long pos)
{
    float pos_buff;

    pos_buff = pos;
    switch(axis)
    {
        case X_AXIS: return (long)(pos_buff*XGearRatio);break;
        case Y_AXIS: return (long)(pos_buff*YGearRatio);break;
        case Z_AXIS: return (long)(pos_buff*ZGearRatio);break;
        default:return (long)(pos_buff*XGearRatio);
    }
}


//设置真实位置
void SetPos(unsigned short axis, long pos)
{
    switch(axis)
    {
        case X_AXIS:
         //if(axis_x_ctrl_mode){//通讯模式
         //   axis_set_com_pos(axis,pos );
          //}
          //else {   
            axis_set_parameter(axis_x, AXIS_POSITION, PosToPulse(axis,pos));
          //}          
          break;
        case Y_AXIS:
         //if(axis_y_ctrl_mode){//通讯模式
         //   axis_set_com_pos(axis,pos );
          //}
         // else {   
            axis_set_parameter(axis_y, AXIS_POSITION, PosToPulse(axis,pos));
          //}         
          break;
        case Z_AXIS:
          //if(axis_z_ctrl_mode){//通讯模式
          //  axis_set_com_pos(axis,pos );
          //}
         // else {   
            axis_set_parameter(axis_z, AXIS_POSITION, PosToPulse(axis,pos));
          //}          
          break;
        default:;
    }
}

//获取真实位置
long GetPos(unsigned short axis)
{
    switch(axis)
    {
        case X_AXIS:
          {
  //          if(axis_x_ctrl_mode){//通讯模式
  //            return get_encode_value(axis);
    //        }
   //         else {//IO模式     
              return  PulseToPos(axis, axis_get_parameter(axis_x, AXIS_POSITION));
       //     }                 
          }break;
        case Y_AXIS:
          {
        //    if(axis_y_ctrl_mode){//通讯模式
    //          return get_encode_value(axis);
       //     }
      //      else {//IO模式     
              return  PulseToPos(axis, axis_get_parameter(axis_y, AXIS_POSITION));
//            }                 
          }break;
        case Z_AXIS:
          {
            //if(axis_z_ctrl_mode){//通讯模式
            //  return get_encode_value(axis);
            //}
            //else {//IO模式     
               return  PulseToPos(axis, axis_get_parameter(axis_z, AXIS_POSITION));
           // }                
          }break;
        default:;
    }
    return 0;
}
//获取当前速度
unsigned long GetCurSpeed(unsigned short axis)
{
    switch(axis)
    {
        case X_AXIS:
          axis_x->cur_speed=axis_x->clock/axis_x->cur_reg_value;
          return axis_x->cur_speed; break;
        case Y_AXIS:
          axis_y->cur_speed=axis_y->clock/axis_y->cur_reg_value;
          return axis_y->cur_speed; break;
        case Z_AXIS:
          axis_z->cur_speed=axis_z->clock/axis_z->cur_reg_value;
          return axis_z->cur_speed; break;
        default:
          axis_x->cur_speed=axis_x->clock/axis_x->cur_reg_value;
          return axis_x->cur_speed; break;
    }
 //   return 0;

}
//获取设置速度
unsigned long GetSetSpeed(unsigned short axis)
{

    return 0;
}

//获取脉冲状态
unsigned long GetState(unsigned short axis)
{
    return 0;

}

//获取脉冲状态
unsigned long SetState(unsigned short axis,unsigned short state)
{

    return 0;
}

//获取剩余脉冲数
unsigned long GetRemainPulse(unsigned short axis)
{
    return 0;
}

//设置脉冲数
unsigned long SetPulse(unsigned short axis,unsigned long pulse)
{
    return 0;
}
//设置起动速度
void SetStartSpeed(unsigned short axis,unsigned short speed)
{
    switch(axis)
    {
        case X_AXIS:axis_set_parameter(axis_x, AXIS_START_SPEED,speed);break;
        case Y_AXIS:axis_set_parameter(axis_y, AXIS_START_SPEED,speed);break;
        case Z_AXIS:axis_set_parameter(axis_z, AXIS_START_SPEED,speed);break;
        default:;
    } 
}

//设置最低速度
void SetMinSpeed(unsigned short axis,unsigned short speed)
{

}

void SetAccTime(unsigned short axis,unsigned short acc_time)
{
    switch(axis)
    {
        case X_AXIS:axis_set_parameter(axis_x, AXIS_ACC_TIME,acc_time);break;
        case Y_AXIS:axis_set_parameter(axis_y, AXIS_ACC_TIME,acc_time);break;
        case Z_AXIS:axis_set_parameter(axis_z, AXIS_ACC_TIME,acc_time);break;
        default:;
    }
}

void SetDecTime(unsigned short axis,unsigned short dec_time)
{
    switch(axis)
    {
        case X_AXIS:axis_set_parameter(axis_x, AXIS_DEC_TIME,dec_time);break;
        case Y_AXIS:axis_set_parameter(axis_y, AXIS_DEC_TIME,dec_time);break;
        case Z_AXIS:axis_set_parameter(axis_z, AXIS_DEC_TIME,dec_time);break;
        default:;
    }
}

void SetStopSelect(unsigned short axis,unsigned short Select_Flag)
{
    switch(axis)
    {
        case X_AXIS:axis_set_parameter(axis_x, AXIS_PP_STOP_SELECT,Select_Flag);break;
        case Y_AXIS:axis_set_parameter(axis_y, AXIS_PP_STOP_SELECT,Select_Flag);break;
        case Z_AXIS:axis_set_parameter(axis_z, AXIS_PP_STOP_SELECT,Select_Flag);break;
        default:;
    } 
}


//运动当中改变速度
void AxisChangeSpeed(unsigned short axis,unsigned short speed)
{
    switch(axis)
    {
        case X_AXIS:
                axis_pv_change_speed(axis_x,speed);

                break;
        case Y_AXIS:
                  axis_pv_change_speed(axis_y,speed);
                   break;
        case Z_AXIS:
                  axis_pv_change_speed(axis_z,speed);
                   break;
        default:;
    }
}

//固定减速距离停止
void AxisDecStopPos(unsigned short axis,unsigned long pos)
{
    switch(axis)
    {
        case X_AXIS: axis_stop_at(axis_x,PosToPulse(X_AXIS,pos));break;
        case Y_AXIS: axis_stop_at(axis_y,PosToPulse(Y_AXIS,pos));break;
        case Z_AXIS: axis_stop_at(axis_z,PosToPulse(Z_AXIS,pos));break;
        default:;
    }
}

//减速停
void AxisDecStop(unsigned short axis)
{
    switch(axis)
    {
        case X_AXIS:

              axis_stop(axis_x);break;
        case Y_AXIS:
              axis_stop(axis_y);break;
        case Z_AXIS:

              axis_stop(axis_z);break;
        default:
            break;
    }
}

//急停
void AxisEgmStop(unsigned short axis)
{
    switch(axis)
    {
        case X_AXIS:
          {
               axis_emgstop(axis_x);
          }break;
        case Y_AXIS:
          {
               axis_emgstop(axis_y);
          }break;
        case Z_AXIS:
          {

               axis_emgstop(axis_z);
          }break;
        default:
            break;
    }
}

//移动绝对位置
void AxisMovePoint(unsigned short axis,unsigned short speed,long pos)
{
    switch(axis)
    {
        case X_AXIS: axis_pp(axis_x, 1, PosToPulse(X_AXIS,pos),speed,0);break;
        case Y_AXIS: axis_pp(axis_y, 1, PosToPulse(Y_AXIS,pos),speed,0);break;
        case Z_AXIS: axis_pp(axis_z, 1, PosToPulse(Z_AXIS,pos),speed,0);break;
        default:;
    }
}

//移动绝对位置带加减速
void AxisMovePointAccDec(unsigned short axis,unsigned short speed,long pos,
      unsigned short start_speed,unsigned short stop_speed,
      unsigned short acc_pulse,unsigned short dec_pulse)
{
    switch(axis)
    {
        case X_AXIS: 
          axis_x->start_speed = start_speed * axis_x->speed_unit;
          axis_x->stop_speed = stop_speed * axis_x->speed_unit;
          axis_x->acc_time = acc_pulse;
          axis_x->dec_time = dec_pulse;
          SetStopSelect(X_AXIS,PP_Stop);
          axis_pp(axis_x, 1, PosToPulse(X_AXIS,pos),speed,0);break;
        case Y_AXIS: 
          axis_y->start_speed = start_speed * axis_y->speed_unit;
          axis_y->stop_speed = stop_speed * axis_y->speed_unit;
          axis_y->acc_time = acc_pulse;
          axis_y->dec_time = dec_pulse;
          SetStopSelect(Y_AXIS,PP_Stop);
          axis_pp(axis_y, 1, PosToPulse(Y_AXIS,pos),speed,0);break;
        case Z_AXIS: 
          axis_z->start_speed = start_speed * axis_z->speed_unit;
          axis_z->stop_speed = stop_speed * axis_z->speed_unit;
          axis_z->acc_time = acc_pulse;
          axis_z->dec_time = dec_pulse;
          SetStopSelect(Z_AXIS,PP_Stop);
          axis_pp(axis_z, 1, PosToPulse(Z_AXIS,pos),speed,0);break;
        default:;
    }
}


//移动距离
void AxisMovePos(unsigned short axis,unsigned short speed,long pos)
{
    switch(axis)
    {
        case X_AXIS:
        {
              SetStopSelect(X_AXIS,PP_Stop);
               axis_pp(axis_x, 0, PosToPulse(X_AXIS,pos),speed,0);

        }break;
        case Y_AXIS:
        {
               axis_pp(axis_y, 0, PosToPulse(Y_AXIS,pos),speed,0);

        }break;
        case Z_AXIS:
        {
              SetStopSelect(Y_AXIS,PP_Stop);
               axis_pp(axis_z, 0, PosToPulse(Z_AXIS,pos),speed,0);

        }break;
        default:;
    }
}

//移动距离
/* 如果运动过程中调用，则start_speed参数无效，使用当前速度作为起始速度*/
void AxisMovePosAccDec(unsigned short axis,unsigned short speed,long pos,
      unsigned short start_speed,unsigned short stop_speed,
      unsigned short acc_pulse,unsigned short dec_pulse,long slowpos)
{
    if(pos==0)return;
    switch(axis)
    {
        case X_AXIS:
        {
            if(axis_x->outputs.on)
            {  
              axis_x->start_speed = GetCurSpeed(axis);
              axis_x->stop_speed = stop_speed * axis_x->speed_unit;
              axis_x->acc_time = acc_pulse;
              axis_x->dec_time = dec_pulse;
              axis_pp_change_speed(axis_x, 0, PosToPulse(X_AXIS,pos),speed,slowpos);
               SetStopSelect(X_AXIS,PP_Stop);
            }
            else
            {
              axis_x->start_speed = start_speed * axis_x->speed_unit;
              axis_x->stop_speed = stop_speed * axis_x->speed_unit;
              axis_x->acc_time = acc_pulse;
              axis_x->dec_time = dec_pulse;
              axis_pp(axis_x, 0, PosToPulse(X_AXIS,pos),speed,slowpos);
              SetStopSelect(X_AXIS,PP_Stop);
            }
        }
        break;
        case Y_AXIS:
        {
            if(axis_y->outputs.on)
            {
              axis_y->start_speed = GetCurSpeed(axis);
              axis_y->stop_speed = stop_speed * axis_y->speed_unit;
              axis_y->acc_time = acc_pulse;
              axis_y->dec_time = dec_pulse;
              axis_pp_change_speed(axis_y, 0, PosToPulse(Y_AXIS,pos),speed,slowpos);
              SetStopSelect(Y_AXIS,PP_Stop);
            }
            else
            {
              axis_y->start_speed = start_speed * axis_y->speed_unit;
              axis_y->stop_speed = stop_speed * axis_y->speed_unit;
              axis_y->acc_time = acc_pulse;
              axis_y->dec_time = dec_pulse;
              axis_pp(axis_y, 0, PosToPulse(Y_AXIS,pos),speed,slowpos);
              SetStopSelect(Y_AXIS,PP_Stop);
            }
          
        }break;
        case Z_AXIS:
        {
            if(axis_z->outputs.on)
            {
              axis_z->start_speed = GetCurSpeed(axis);
              axis_z->stop_speed = stop_speed * axis_z->speed_unit;
              axis_z->acc_time = acc_pulse;
              axis_z->dec_time = dec_pulse;
              axis_pp_change_speed(axis_z, 0, PosToPulse(Z_AXIS,pos),speed,slowpos);
              SetStopSelect(Z_AXIS,PP_Stop);
            }
            else
            {
              axis_z->start_speed = start_speed * axis_z->speed_unit;
              axis_z->stop_speed = stop_speed * axis_z->speed_unit;
              axis_z->acc_time = acc_pulse;
              axis_z->dec_time = dec_pulse;
              axis_pp(axis_z, 0, PosToPulse(Z_AXIS,pos),speed,slowpos);
              SetStopSelect(Z_AXIS,PP_Stop);
            }
          }
        break;
        default:;
    }
}

//移动距离不停止
/*
   参数
    axis        轴选择
    speed       最高速度位置
    pos         移动位置
    start_speed 启动速度,如果运动过程中调用，则start_speed参数无效，使用当前速度作为起始速度
    acc_pulse   加速度
    dec_pulse   减速度
    lastspeed   数控最后速度  
    slowpos 
*/

void AxisMovePosAccDecNotStop(unsigned short axis,unsigned short speed,long pos,
    unsigned short start_speed,unsigned short lastspeed,
    unsigned short acc_pulse,unsigned short dec_pulse,long slowpos)
{
    if(pos==0)return;
    switch(axis)
    {
        case X_AXIS:
        {
            if(axis_x->outputs.on)
            {
              SetStopSelect(X_AXIS,PP_NoStop);
              axis_x->start_speed = GetCurSpeed(axis);;
              axis_x->stop_speed = lastspeed * axis_x->speed_unit;
              axis_x->acc_time = acc_pulse;
              axis_x->dec_time = dec_pulse;
              axis_pp_change_speed(axis_x, 0, PosToPulse(X_AXIS,pos),speed,slowpos);
            }
            else
            {
              SetStopSelect(X_AXIS,PP_NoStop);
              axis_x->start_speed = start_speed * axis_x->speed_unit;
              axis_x->stop_speed = lastspeed * axis_x->speed_unit;
              axis_x->acc_time = acc_pulse;
              axis_x->dec_time = dec_pulse;
              axis_pp(axis_x, 0, PosToPulse(X_AXIS,pos),speed,slowpos);
            }
        }
        break;
        case Y_AXIS:
        {
            if(axis_y->outputs.on)
            {
              SetStopSelect(Y_AXIS,PP_NoStop);
              axis_y->start_speed = GetCurSpeed(axis);;
              axis_y->stop_speed = lastspeed * axis_y->speed_unit;
              axis_y->acc_time = acc_pulse;
              axis_y->dec_time = dec_pulse;
              axis_pp_change_speed(axis_y, 0, PosToPulse(Y_AXIS,pos),speed,slowpos);
            }
            else
            {
              SetStopSelect(Y_AXIS,PP_NoStop);
              axis_y->start_speed = start_speed * axis_y->speed_unit;
              axis_y->stop_speed = lastspeed * axis_y->speed_unit;
              axis_y->acc_time = acc_pulse;
              axis_y->dec_time = dec_pulse;
              axis_pp(axis_y, 0, PosToPulse(Y_AXIS,pos),speed,slowpos);
            }
          
        }break;
        case Z_AXIS:
        {
            if(axis_z->outputs.on)
            {
              SetStopSelect(Z_AXIS,PP_NoStop);
              axis_z->start_speed = GetCurSpeed(axis);
              axis_z->stop_speed = lastspeed * axis_z->speed_unit;
              axis_z->acc_time = acc_pulse;
              axis_z->dec_time = dec_pulse;
              axis_pp_change_speed(axis_z, 0, PosToPulse(Z_AXIS,pos),speed,slowpos);
            }
            else
            {
              SetStopSelect(Z_AXIS,PP_NoStop);
              axis_z->start_speed = start_speed * axis_z->speed_unit;
              axis_z->stop_speed = lastspeed * axis_z->speed_unit;
              axis_z->acc_time = acc_pulse;
              axis_z->dec_time = dec_pulse;
              axis_pp(axis_z, 0, PosToPulse(Z_AXIS,pos),speed,slowpos);
            }
          }
        break;
        default:;
    }             	
}

//连续运动
void AxisContinueMove(unsigned short axis,unsigned short speed,unsigned char dir)
{
    switch(axis)
    {
        case X_AXIS:
        {
             axis_pv(axis_x,dir,speed);

        }break;
        case Y_AXIS:
        {
              axis_pv(axis_y,dir,speed);

        }break;
        case Z_AXIS:
        {

               axis_pv(axis_z,dir,speed);

        }break;
        default:;
    }
}

//连续运动带加减速参数
void AxisContinueMoveAcc(unsigned short axis,unsigned short speed,unsigned char dir,
      unsigned short start_speed,unsigned short stop_speed,
      unsigned short acc,unsigned short dec)
{
    switch(axis)
    {
        case X_AXIS:
        {
              axis_x->start_speed = start_speed * axis_x->speed_unit;
              axis_x->stop_speed = stop_speed * axis_x->speed_unit;
              axis_x->acc_time = acc;
              axis_x->dec_time = dec;
              axis_pv(axis_x,dir,speed);
        }break;
        case Y_AXIS:
        {
              axis_y->start_speed = start_speed * axis_y->speed_unit;
              axis_y->stop_speed = stop_speed * axis_y->speed_unit;
              axis_y->acc_time = acc;
              axis_y->dec_time = dec;
              axis_pv(axis_y,dir,speed);
        }break;
        case Z_AXIS:
        {
              axis_z->start_speed = start_speed * axis_z->speed_unit;
              axis_z->stop_speed = stop_speed * axis_z->speed_unit;
              axis_z->acc_time = acc;
              axis_z->dec_time = dec;
              axis_pv(axis_z,dir,speed);
        }break;
        default:;
    }
}

//连续运动带加减速参数--中途改变目标速度
void AxisContinueMoveChangeSpeed(unsigned short axis,unsigned short speed,unsigned short stop_speed,
      unsigned short acc,unsigned short dec)
{
    GetCurSpeed(axis);
    switch(axis)
    {
        case X_AXIS:
        {
            if(axis_x->outputs.on)
            {
              axis_x->start_speed = GetCurSpeed(axis);;
              axis_x->stop_speed = stop_speed * axis_x->speed_unit;
              axis_x->acc_time = acc;
              axis_x->dec_time = dec;
              axis_pv_change_speed_table(axis_x,speed);
            }
        }break;
        case Y_AXIS:
        {
            if(axis_y->outputs.on)
            {
              axis_y->start_speed =  GetCurSpeed(axis);;
              axis_y->stop_speed = stop_speed * axis_y->speed_unit;
              axis_y->acc_time = acc;
              axis_y->dec_time = dec;
              axis_pv_change_speed_table(axis_y,speed);
            }
        }break;
        case Z_AXIS:
        {
            if(axis_z->outputs.on)
            {
              axis_z->start_speed = GetCurSpeed(axis);;
              axis_z->stop_speed = stop_speed * axis_z->speed_unit;
              axis_z->acc_time = acc;
              axis_z->dec_time = dec;
              axis_pv_change_speed_table(axis_z,speed);
            }
        }break;
        default:;
    }
}

//直接变速不经过加减速过程，支持PP、PV模式调用，调用后切换到连续运动PV模式
void AxisChangeSpeedDirect(unsigned short axis,unsigned short speed)
{
    switch(axis)
    {
        case X_AXIS:
        {
            if(axis_x->outputs.on)
            {
              axis_change_speed_direct(axis_x,speed);      
            }
        }break;
        case Y_AXIS:
        {
            if(axis_y->outputs.on)
            {
              axis_change_speed_direct(axis_y,speed);
            }
        }break;
        case Z_AXIS:
        {
            if(axis_z->outputs.on)
            {
              axis_change_speed_direct(axis_z,speed);
            }
        }break;
        default:;
    }
}



//两段速度移动距离
void AxisMoveTwoPos(unsigned short axis,unsigned short speed1,long pos1,
                    unsigned short speed2,long pos2,int dir,
                    unsigned short start_speed,unsigned short stop_speed,
                    unsigned short acc_pulse,unsigned short dec_pulse)
{
    switch(axis)
    {
        case X_AXIS:
        {
                axis_init_task(axis_x,dir);
                SetStopSelect(X_AXIS,PP_Stop);
                axis_x->start_speed = start_speed * axis_x->speed_unit;
                axis_x->stop_speed = stop_speed * axis_x->speed_unit;
                axis_x->acc_time = acc_pulse;
                axis_x->dec_time = dec_pulse;
                axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos1), speed1);
                axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos2), speed2);
                axis_start_pp_task(axis_x);
            }
        break;
        case Y_AXIS:
        {
                SetStopSelect(Y_AXIS,PP_Stop);
                axis_init_task(axis_y,dir);
                axis_y->start_speed =  start_speed * axis_y->speed_unit;
                axis_y->stop_speed = stop_speed * axis_y->speed_unit;
                axis_y->acc_time = acc_pulse;
                axis_y->dec_time = dec_pulse;
                axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos1), speed1);
                axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos2), speed2);
                axis_start_pp_task(axis_y);
            
        }
        break;
      case Z_AXIS:
        {
                SetStopSelect(Z_AXIS,PP_Stop);
                axis_init_task(axis_z,dir);
                axis_z->start_speed = start_speed * axis_z->speed_unit;
                axis_z->stop_speed = stop_speed * axis_z->speed_unit;
                axis_z->acc_time = acc_pulse;
                axis_z->dec_time = dec_pulse;
                axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos1), speed1);
                axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos2), speed2);
                axis_start_pp_task(axis_z);
        }
        break;
        default:
          break;
    }
}
void AxisMoveTwoPosNoStop(unsigned short axis,unsigned short speed1,long pos1,
                    unsigned short speed2,long pos2,int dir,
                    unsigned short start_speed,unsigned short stop_speed,
                    unsigned short acc_pulse,unsigned short dec_pulse)
{
    switch(axis)
    {
        case X_AXIS:
        {
                axis_init_task(axis_x,dir);
                SetStopSelect(X_AXIS,PP_NoStop);
                axis_x->start_speed = start_speed * axis_x->speed_unit;
                axis_x->stop_speed = stop_speed * axis_x->speed_unit;
                axis_x->acc_time = acc_pulse;
                axis_x->dec_time = dec_pulse;
                axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos1), speed1);
                axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos2), speed2);
                axis_start_pp_task(axis_x);
            }
        break;
        case Y_AXIS:
        {
                SetStopSelect(Y_AXIS,PP_NoStop);
                axis_init_task(axis_y,dir);
                axis_y->start_speed =  start_speed * axis_y->speed_unit;
                axis_y->stop_speed = stop_speed * axis_y->speed_unit;
                axis_y->acc_time = acc_pulse;
                axis_y->dec_time = dec_pulse;
                axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos1), speed1);
                axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos2), speed2);
                axis_start_pp_task(axis_y);
        }
        break;
      case Z_AXIS:
        {
                SetStopSelect(Z_AXIS,PP_NoStop);
                axis_init_task(axis_z,dir);
                axis_z->start_speed = start_speed * axis_z->speed_unit;
                axis_z->stop_speed = stop_speed * axis_z->speed_unit;
                axis_z->acc_time = acc_pulse;
                axis_z->dec_time = dec_pulse;
                axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos1), speed1);
                axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos2), speed2);
                axis_start_pp_task(axis_z);
        }
        break;
        default:
          break;
    }
}

//三段速度移动距离
void AxisMoveThreePos(unsigned short axis,unsigned short speed1,long pos1,unsigned short speed2,long pos2,unsigned short speed3,long pos3,int dir)
{
    switch(axis)
    {
        case X_AXIS:
        {
            axis_init_task(axis_x,dir);
            SetStopSelect(X_AXIS,PP_Stop);
            axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos1), speed1);
            axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos2), speed2);
            axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos3), speed3);
            axis_start_pp_task(axis_x);
        }break;
        case Y_AXIS:
        {
            axis_init_task(axis_y,dir);
            SetStopSelect(Y_AXIS,PP_Stop);
            axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos1), speed1);
            axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos2), speed2);
            axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos3), speed3);
            axis_start_pp_task(axis_y);
        }
        break;
        case Z_AXIS:
        {
            axis_init_task(axis_z,dir);
            SetStopSelect(Z_AXIS,PP_Stop);
            axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos1), speed1);
            axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos2), speed2);
            axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos3), speed3);
            axis_start_pp_task(axis_z);
        }
        break;
        default:;
    }
}

//三段速度移动距离不停止
void AxisMoveThreePosNoStop(unsigned short axis,unsigned short speed1,long pos1,unsigned short speed2,long pos2,unsigned short speed3,long pos3,int dir)
{
    switch(axis)
    {
        case X_AXIS:
        {
            axis_init_task(axis_x,dir);
            SetStopSelect(X_AXIS,PP_NoStop);
            axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos1), speed1);
            axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos2), speed2);
            axis_add_pp_task(axis_x, PosToPulse(X_AXIS,pos3), speed3);
            axis_start_pp_task(axis_x);    
        }break;
        case Y_AXIS:
        {
            axis_init_task(axis_y,dir);
            SetStopSelect(Y_AXIS,PP_NoStop);
            axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos1), speed1);
            axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos2), speed2);
            axis_add_pp_task(axis_y, PosToPulse(Y_AXIS,pos3), speed3);
            axis_start_pp_task(axis_y);           
        }break;
        case Z_AXIS:
        {
            axis_init_task(axis_z,dir);
            SetStopSelect(Z_AXIS,PP_NoStop);
            axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos1), speed1);
            axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos2), speed2);
            axis_add_pp_task(axis_z, PosToPulse(Z_AXIS,pos3), speed3);
            axis_start_pp_task(axis_z);           
        }break;
        default:;
    }
}
void AxisEnableSoftwarelmt(unsigned short axis, int p_pos, int n_pos)
{
    switch(axis)
    {
    case X_AXIS:
           axis_enable_software_lmt(axis_x, PosToPulse(X_AXIS,p_pos), PosToPulse(X_AXIS,n_pos));
        break;
    case Y_AXIS:
           axis_enable_software_lmt(axis_y, PosToPulse(Y_AXIS,p_pos), PosToPulse(Y_AXIS,n_pos));        
        break;
    case Z_AXIS:
           axis_enable_software_lmt(axis_z, PosToPulse(Z_AXIS,p_pos), PosToPulse(Z_AXIS,n_pos));         
        break;
    default:
        break;
    }
}
void AxisDisableSoftwarelmt(unsigned short axis)
{
    switch(axis)
    {
    case X_AXIS:
           axis_disable_software_lmt(axis_x);           
        break;
    case Y_AXIS:
           axis_disable_software_lmt(axis_y);                   
        break;
    case Z_AXIS:
           axis_disable_software_lmt(axis_z);                   
        break;
    default:
      break;
    }
}