#include "global.h"
#include "servocom_app.h"
servo_param_t servo_x,servo_y;
//伺服通讯命令反馈。
char servo_com_cmd_fb[SERVO_COM_CMD_NUMBER+1];
//华创伺服参数读取列表
servo_com_cmd_t servo_com_cmd_table[SERVO_AXISNUM][SERVO_COM_CMD_NUMBER] ={{
  //命令编码-------目标地址-------------通讯个数--------------数值地址------------------ID--------------CMD-cycle-time---回调函数
  {HANDSHAKE,      SERVO_ADDR_HAND,         1,(unsigned char *)&servo_x.handshake,       SERVO_X_COM_ID,0x03,0,50,servo_com_respone},
  {DI_MODE_SW,     SERVO_ADDR_DI_MODE_SW,   2,(unsigned char *)&servo_x.di_function_set, SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {DO_TARR_SW,     SERVO_ADDR_DO_TARR_OV,   2,(unsigned char *)&servo_x.do_function_set, SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {CTRL_MODE_SET,  SERVO_ADDR_MODE_SET,     1,(unsigned char *)&servo_x.ctrl_mode_set,   SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {MOTOR_DIR_SET,  SERVO_ADDR_MOTOR_DIR,    1,(unsigned char *)&servo_x.motor_dir_set,   SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {TARR_SOURCE_SEL,SERVO_ADDR_TARR_SEL,     14,(unsigned char *)&servo_x.tarr_source_sel,SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {TARR_LIMIT12_SET, SERVO_ADDR_LIMIT_SET,  9,(unsigned char *)&servo_x.tarr_Plimit1_sel,SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {SPEED_LIMIT_SET,SERVO_ADDR_SPEED_SET,    3,(unsigned char *)&servo_x.speed_plimit_set,SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {TARR_BASE_SET, SERVO_ADDR_TARBASE_SET,   3,(unsigned char *)&servo_x.tarr_basis,      SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {ALARM_REST,     SERVO_ADDR_ALARM_REST,   1,(unsigned char *)&servo_x.alarmrest,       SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {SERVO_RESTART,  SERVO_ADDR_SERVO_RESTART,1,(unsigned char *)&servo_x.restart,         SERVO_X_COM_ID,0x06,0,50,servo_com_respone},
  {ALARM_CODE,     SERVO_ADDR_ALARM_CODE,   1,(unsigned char *)&servo_x.alarmcode,       SERVO_X_COM_ID,0x03,1,50,servo_com_respone},
  {ENCODE_FB,      SERVO_ADDR_ENCODE_FB,    2,(unsigned char *)&servo_x.encode,          SERVO_X_COM_ID,0x03,1,50,servo_com_respone}
},
{
  {HANDSHAKE,      SERVO_ADDR_HAND,         1,(unsigned char *)&servo_y.handshake,       SERVO_Y_COM_ID,0x03,0,50,servo_com_respone},
  {DI_MODE_SW,     SERVO_ADDR_DI_MODE_SW,   2,(unsigned char *)&servo_y.di_function_set, SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {DO_TARR_SW,     SERVO_ADDR_DO_TARR_OV,   2,(unsigned char *)&servo_y.do_function_set, SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {CTRL_MODE_SET,  SERVO_ADDR_MODE_SET,     1,(unsigned char *)&servo_y.ctrl_mode_set,   SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {MOTOR_DIR_SET,  SERVO_ADDR_MOTOR_DIR,    1,(unsigned char *)&servo_y.motor_dir_set,   SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {TARR_SOURCE_SEL,SERVO_ADDR_TARR_SEL,     14,(unsigned char *)&servo_y.tarr_source_sel,SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {TARR_LIMIT12_SET, SERVO_ADDR_LIMIT_SET,  9,(unsigned char *)&servo_y.tarr_Plimit1_sel,SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {SPEED_LIMIT_SET,SERVO_ADDR_SPEED_SET,    3,(unsigned char *)&servo_y.speed_plimit_set,SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {TARR_BASE_SET, SERVO_ADDR_TARBASE_SET,   3,(unsigned char *)&servo_y.tarr_basis,      SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {ALARM_REST,     SERVO_ADDR_ALARM_REST,   1,(unsigned char *)&servo_y.alarmrest,       SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {SERVO_RESTART,  SERVO_ADDR_SERVO_RESTART,1,(unsigned char *)&servo_y.restart,         SERVO_Y_COM_ID,0x06,0,50,servo_com_respone},
  {ALARM_CODE,     SERVO_ADDR_ALARM_CODE,   1,(unsigned char *)&servo_y.alarmcode,       SERVO_Y_COM_ID,0x03,1,50,servo_com_respone},
  {ENCODE_FB,      SERVO_ADDR_ENCODE_FB,    2,(unsigned char *)&servo_y.encode,          SERVO_Y_COM_ID,0x03,1,50,servo_com_respone}
}
};

/*
 * 伺服通讯命令函数
 * 
 * @author laz
 * 
 * @param cmd 
*/
static void servo_com_cmd(unsigned short axis,unsigned char cmd)
{
  int i,cyckletime;
  switch(axis)
  {
  case X_AXIS:
    for(i=0;i<SERVO_COM_CMD_NUMBER;i++)
    {
      if(cmd==servo_com_cmd_table[0][i].cmdcode){
        break;
      }
    }
    if(servo_com_cmd_table[0][i].dir==0) {//单次通讯
      servo_x.cmd_fb[servo_com_cmd_table[0][i].cmdcode]=0;
      //添加单次采集命令
      modbus_master_add_once_cmd(
      &master1,
      servo_com_cmd_table[0][i].cmdcode,         //命令编码
      servo_com_cmd_table[0][i].id,              //设备ID
      servo_com_cmd_table[0][i].cmd,             //命令码
      servo_com_cmd_table[0][i].address,         //10
      servo_com_cmd_table[0][i].number,          //10
      servo_com_cmd_table[0][i].value,           //数据存放位置
      servo_com_cmd_table[0][i].respone_timeout, //50ms超时时间（不包括通讯必须时间）
      servo_com_cmd_table[0][i].servo_on_respone //不需要回调函数
      );  
    }
    else
    {
      if(cmd==ENCODE_FB)cyckletime=100;
      else cyckletime=1000;      
      //添加循环采集命令
      modbus_master_add_period_cmd(
      &master1,
      servo_com_cmd_table[0][i].cmdcode,         //命令编码
      servo_com_cmd_table[0][i].id,              //设备ID
      servo_com_cmd_table[0][i].cmd,             //命令码
      servo_com_cmd_table[0][i].address,         //10
      servo_com_cmd_table[0][i].number,          //10
      servo_com_cmd_table[0][i].value,           //数据存放位置
      servo_com_cmd_table[0][i].respone_timeout, //50ms超时时间（不包括通讯必须时间）
      servo_com_cmd_table[0][i].servo_on_respone,//不需要回调函数
      cyckletime   //1000ms采集一次
      ); 
    }
  break;
  case Y_AXIS:
    for(i=0;i<SERVO_COM_CMD_NUMBER;i++)
    {
      if(cmd==servo_com_cmd_table[1][i].cmdcode){
        break;
      }
    }
    if(servo_com_cmd_table[1][i].dir==0) {//单次通讯
      servo_y.cmd_fb[servo_com_cmd_table[0][i].cmdcode]=0;
      //添加单次采集命令
      modbus_master_add_once_cmd(
      &master1,
      servo_com_cmd_table[1][i].cmdcode,         //命令编码
      servo_com_cmd_table[1][i].id,              //设备ID
      servo_com_cmd_table[1][i].cmd,             //命令码
      servo_com_cmd_table[1][i].address,         //10
      servo_com_cmd_table[1][i].number,          //10
      servo_com_cmd_table[1][i].value,           //数据存放位置
      servo_com_cmd_table[1][i].respone_timeout, //50ms超时时间（不包括通讯必须时间）
      servo_com_cmd_table[1][i].servo_on_respone //不需要回调函数
      );  
    }
    else
    {
      if(cmd==ENCODE_FB)cyckletime=100;
      else cyckletime=1000;      
      //添加循环采集命令
      modbus_master_add_period_cmd(
      &master1,
      servo_com_cmd_table[1][i].cmdcode,         //命令编码
      servo_com_cmd_table[1][i].id,              //设备ID
      servo_com_cmd_table[1][i].cmd,             //命令码
      servo_com_cmd_table[1][i].address,         //10
      servo_com_cmd_table[1][i].number,          //10
      servo_com_cmd_table[1][i].value,           //数据存放位置
      servo_com_cmd_table[1][i].respone_timeout, //50ms超时时间（不包括通讯必须时间）
      servo_com_cmd_table[1][i].servo_on_respone,//不需要回调函数
      cyckletime   //1000ms采集一次
      ); 
    }
    break;
  }
}
/*
 * 伺服通讯，故障清除函数
 * 
 * @author laz
 * 
 * @param aixs 
*/
int clr_com_servo_alarm(unsigned short axis)
{
  static unsigned long clr_com_servo_x_delay=0;
  static unsigned long clr_com_servo_y_delay=0;
  switch(axis)
  {
  case X_AXIS:
    if(dwTickCount>=clr_com_servo_x_delay)
    {
       servo_x.alarmrest=1;       
       clr_com_servo_x_delay=dwTickCount+1000;
       servo_com_cmd(axis,ALARM_REST);
    }
    break;
  case Y_AXIS:
    if(dwTickCount>=clr_com_servo_y_delay)
    {
       servo_y.alarmrest=1; 
       servo_com_cmd(axis,ALARM_REST);
       clr_com_servo_y_delay=dwTickCount+1000;
    }
    break;
  }
  return 1;
}
/*
 * 伺服通讯，设置细分，设置周长
 * 
 * @author laz
 * 
 * @param aixs 
*/
void set_com_servo_param(unsigned short axis,unsigned short cycle_pluse,unsigned short cycle_length)
{
  switch(axis)
  {
  case X_AXIS:
    servo_x.cycle_pulse=cycle_pluse;
    servo_x.cycle_length=cycle_length;
    break;
  case Y_AXIS:
    servo_y.cycle_pulse=cycle_pluse;
    servo_y.cycle_length=cycle_length;
    break;
  }
}
/*
 * 伺服通讯，设置伺服告警极性
 * 
 * @author laz
 * 
 * @param aixs 
*/
void set_com_alarm_reverse(unsigned short axis,int value)
{
   switch(axis)
  {
  case X_AXIS:
    servo_x.alarm_Reverse=value;
    break;
  case Y_AXIS:
    servo_y.alarm_Reverse=value;
    break;
  }
}
/*
 * 伺服通讯，获取伺服故障代码
 * 
 * @author laz
 * 
 * @param aixs 
*/
int get_alarm_code(unsigned short axis)
{
   switch(axis)
  {
  case X_AXIS:
    return (servo_x.alarmcode);
    break;
  case Y_AXIS:
    return (servo_y.alarmcode);
    break;
  }
  return 0;
}
/*
 * 伺服通讯，编码转换长度计算
 * 
 * @author laz
 * 
 * @param encode 伺服编码
 * gearratio，齿轮比
*/
static long encode_to_length(long encode,unsigned short cycle_length)
{
  float pos;
  pos=encode;
  pos=(float)(pos/ENCODE_17BIT);
  pos*= cycle_length;
  return (long)(pos);
}
/*
 * 伺服通讯，位置设置值
 * 
 * @author laz
 * 
 * @param axis
 * pos，
*/
int axis_set_com_pos(unsigned short axis,long pos )
{
  switch(axis)
  {
  case X_AXIS:
    servo_x.precode=servo_x.encode-pos;
    break;
  case Y_AXIS:
    servo_y.precode=servo_y.encode-pos;
    break;
  }
  return 1;
}
/*
 * 伺服通讯，长度值
 * 
 * @author laz
 * 
 * @param axis
*/
static long axis_get_com_pos(unsigned short axis)
{
   switch(axis)
  {
  case X_AXIS:
     if(servo_x.encode>=servo_x.precode)
        return(encode_to_length(servo_x.encode-servo_x.precode,servo_x.cycle_length));
     else
        return(encode_to_length(servo_x.precode-servo_x.encode,servo_x.cycle_length));
    break;
  case Y_AXIS:
      if(servo_y.encode>=servo_y.precode)
         return(encode_to_length(servo_y.encode-servo_y.precode,servo_y.cycle_length));
      else
         return(encode_to_length(servo_y.precode-servo_y.encode,servo_y.cycle_length));
    break;
  }
  return 0;
}
/*
 * 伺服通讯，//频率转换为转速
 * 
 * @author laz
 * 
 * @param freq 伺服的频率
 * rerurn 伺服的转速
*/
static unsigned short freq_to_speed(unsigned short freq,unsigned short cycle_pulse)
{
  if(freq==0)return 1;
  if(cycle_pulse==0)return 1;
  return(freq*60/cycle_pulse+1);
}
/*
 * 伺服通讯，读取编码次数
 * 
 * @author laz
 * 
 * @param axis
 * times 读取次数
*/
void read_encode_value_cmd(unsigned short axis,unsigned short times)
{
   int i=0;
   switch(axis)
  {
  case X_AXIS:        
      //servo_x.cmd_fb[ENCODE_FB]=0;
      for(i=0;i<times;i++)servo_com_cmd(axis,ENCODE_FB);
    break;
  case Y_AXIS:
      //servo_y.cmd_fb[ENCODE_FB]=0;
      for(i=0;i<times;i++)servo_com_cmd(axis,ENCODE_FB);
    break;
  }
}
/*
 * 伺服通讯，读取长度值
 * 
 * @author laz
 * 
 * @param axis
*/
long get_encode_value(unsigned short axis)
{
   switch(axis)
  {
  case X_AXIS:        
    return axis_get_com_pos(axis);
    break;
  case Y_AXIS:
    return axis_get_com_pos(axis);
    break;
  }
  return 0;
}
/*
 * 伺服通讯，伺服轴通讯回调函数
 * 
 * @author laz
 * 
 * 
*/
static void servo_axis_com_respone(unsigned short axis,servo_param_t *servo_axis,modbus_master_cmd_t *cmd, unsigned char *respone, int length)
{
  switch(cmd->cmdcode)
  {
  case HANDSHAKE:
    servo_axis->cmd_fb[HANDSHAKE]=cmd->cmdcode;
    break;
  case DI_MODE_SW:
    servo_axis->cmd_fb[DI_MODE_SW]=cmd->cmdcode;
    break;
  case DO_TARR_SW:
    servo_axis->cmd_fb[DO_TARR_SW]=cmd->cmdcode;
    break;
  case CTRL_MODE_SET:
    servo_axis->cmd_fb[CTRL_MODE_SET]=cmd->cmdcode;
    break;
  case MOTOR_DIR_SET:
    servo_axis->cmd_fb[MOTOR_DIR_SET]=cmd->cmdcode;
    break;
  case TARR_SOURCE_SEL:
    servo_axis->cmd_fb[TARR_SOURCE_SEL]=cmd->cmdcode;
    break;
  case TARR_LIMIT12_SET:
    servo_axis->set_tarr_OKflag++;
    servo_axis->cmd_fb[TARR_LIMIT12_SET]=cmd->cmdcode;
    break;
  case SPEED_LIMIT_SET:
    servo_axis->set_tarr_OKflag++;
    servo_axis->cmd_fb[SPEED_LIMIT_SET]=cmd->cmdcode;
    break;
  case TARR_BASE_SET:
    servo_axis->set_tarr_OKflag++;
    servo_axis->cmd_fb[TARR_BASE_SET]=cmd->cmdcode;
    break;
  case ALARM_REST:
    servo_axis->cmd_fb[ALARM_REST]=cmd->cmdcode;
    break;
  case SERVO_RESTART:
    servo_axis->cmd_fb[SERVO_RESTART]=cmd->cmdcode;
    break;
  case ALARM_CODE:
    servo_axis->cmd_fb[ALARM_CODE]=cmd->cmdcode;
    if(servo_axis->alarmcode>0){
      if(servo_axis->alarm_Reverse)servo_axis->alarm_flag=0;else servo_axis->alarm_flag=1;      
    }
    else { 
      if(servo_axis->alarm_Reverse)servo_axis->alarm_flag=1;else servo_axis->alarm_flag=0;  
    }        
    break;
  case ENCODE_FB:
    servo_axis->cmd_fb[ENCODE_FB]=cmd->cmdcode;
    get_encode_value(axis);
    break;  
  default:
    break;
  }
}
static int servo_com_get_alarm(unsigned short axis)
{
  switch(axis)
  {
  case X_AXIS:   
    axis_x_com_alarm=servo_x.alarm_flag;
    break;
  case Y_AXIS:
    axis_y_com_alarm=servo_y.alarm_flag;
    break;
  }
  return 1;
}
/*
 * 伺服通讯，通讯回调函数
 * 
 * @author laz
 * 
 * 
*/
void servo_com_respone(modbus_master_cmd_t *cmd, unsigned char *respone, int length)
{
#if  X_USERING_TARR == 1
  if(cmd->id==SERVO_X_COM_ID)servo_axis_com_respone(X_AXIS,&servo_x,cmd,respone,length);
#endif
#if  Y_USERING_TARR == 1
  if(cmd->id==SERVO_Y_COM_ID)servo_axis_com_respone(Y_AXIS,&servo_y,cmd,respone,length);
#endif 

}

//设置位置+转矩模式下的转矩配置
int set_servo_postotarr_limit(unsigned short axis,int tarr,
                              unsigned short P_freq,unsigned short N_freq,
                              unsigned short dir)
{
     switch(axis)
    {
    case X_AXIS:        
      //配置扭矩模式下的速度限制、扭矩限制、扭矩基准值
      servo_x.speed_plimit_set=freq_to_speed(P_freq,servo_x.cycle_pulse);
      servo_x.speed_nlimit_set=freq_to_speed(N_freq,servo_x.cycle_pulse);
      if(tarr<100)tarr=100;
      if(dir)servo_x.tarr_limit_set=-tarr; 
      else servo_x.tarr_limit_set=tarr;      
      servo_com_cmd(axis,SPEED_LIMIT_SET);
      //servo_x.cmd_fb[SPEED_LIMIT_SET]=0;//清除反馈命令   
      //转矩的基准值设置
      servo_x.tarr_basis=tarr-50;
      servo_x.tarr_basis_up=50;
      servo_x.tarr_basis_dw=30;
      servo_com_cmd(axis,TARR_BASE_SET);
      //servo_x.cmd_fb[TARR_BASE_SET]=0;//清除反馈命令
      servo_x.set_tarr_OKflag=0;
      sw_timer_start(&servo_x.timer, 1, 0);//1S超时
      break;
    case Y_AXIS:
      //配置扭矩模式下的速度限制、扭矩限制、扭矩基准值
      servo_y.speed_plimit_set=freq_to_speed(P_freq,servo_y.cycle_pulse);
      servo_y.speed_nlimit_set=freq_to_speed(N_freq,servo_y.cycle_pulse);
      if(tarr<100)tarr=100;
      if(dir)servo_y.tarr_limit_set=-tarr; 
      else servo_y.tarr_limit_set=tarr;       
      servo_com_cmd(axis,SPEED_LIMIT_SET);
      //servo_y.cmd_fb[SPEED_LIMIT_SET]=0;//清除反馈命令   
      //转矩的基准值设置
      servo_y.tarr_basis=tarr-50;
      servo_y.tarr_basis_up=50;
      servo_y.tarr_basis_dw=30;
      servo_com_cmd(axis,TARR_BASE_SET);
      //servo_y.cmd_fb[TARR_BASE_SET]=0;//清除反馈命令
      servo_y.set_tarr_OKflag=0;
      sw_timer_start(&servo_y.timer, 1, 0);//1S超时
      break;
    }
    return 1;
}
//转矩模式下的扭矩配置
//设置位置+转矩模式下的转矩配置
int set_servo_tarr_limit(unsigned short axis,int maxtarr,int mintarr,
                         unsigned short P_freq,unsigned short N_freq,
                         unsigned short dir)
{
     switch(axis)
    {
    case X_AXIS:        
      if(maxtarr<400)maxtarr=400;
      if(maxtarr>=2000)maxtarr=2000;
      //扭矩1，扭矩2的配置
      servo_x.tarr_Plimit1_sel=mintarr;
      servo_x.tarr_Nlimit1_sel=mintarr;
      servo_x.tarr_Plimit2_sel=maxtarr;
      servo_x.tarr_Nlimit2_sel=maxtarr;
      servo_x.speed_limit_sel=0;
      servo_x.speed_ai_sel=1;
      //配置扭矩模式下的速度限制、扭矩基准值
      servo_x.speed_plimit_set=freq_to_speed(P_freq,servo_x.cycle_pulse);
      servo_x.speed_nlimit_set=freq_to_speed(N_freq,servo_x.cycle_pulse);
      if(dir)servo_x.tarr_limit_set=-(maxtarr+100); 
      else servo_x.tarr_limit_set=(maxtarr+100); 
      servo_com_cmd(axis,TARR_LIMIT12_SET);
      //servo_x.cmd_fb[TARR_LIMIT12_SET]=0;//清除反馈命令   
      //转矩的基准值设置
      servo_x.tarr_basis=mintarr-50;
      servo_x.tarr_basis_up=50;
      servo_x.tarr_basis_dw=30;
      servo_com_cmd(axis,TARR_BASE_SET);
      //servo_x.cmd_fb[TARR_BASE_SET]=0;//清除反馈命令
      servo_x.set_tarr_OKflag=0;
      sw_timer_start(&servo_x.timer, 1, 0);//1S超时
      break;
    case Y_AXIS:
      if(maxtarr<400)maxtarr=400;
      if(maxtarr>=2000)maxtarr=2000;
      //扭矩1，扭矩2的配置
      servo_y.tarr_Plimit1_sel=mintarr;
      servo_y.tarr_Nlimit1_sel=mintarr;
      servo_y.tarr_Plimit2_sel=maxtarr;
      servo_y.tarr_Nlimit2_sel=maxtarr;
      servo_y.speed_limit_sel=0;
      servo_y.speed_ai_sel=1;
      //配置扭矩模式下的速度限制、扭矩基准值
      servo_y.speed_plimit_set=freq_to_speed(P_freq,servo_y.cycle_pulse);
      servo_y.speed_nlimit_set=freq_to_speed(N_freq,servo_y.cycle_pulse);
      if(dir)servo_y.tarr_limit_set=-(maxtarr+100); 
      else servo_y.tarr_limit_set=(maxtarr+100); 
      servo_com_cmd(axis,TARR_LIMIT12_SET);
      //servo_y.cmd_fb[TARR_LIMIT12_SET]=0;//清除反馈命令   
      //转矩的基准值设置
      servo_y.tarr_basis=mintarr-50;
      servo_y.tarr_basis_up=50;
      servo_y.tarr_basis_dw=30;
      servo_com_cmd(axis,TARR_BASE_SET);
      //servo_y.cmd_fb[TARR_BASE_SET]=0;//清除反馈命令
      servo_y.set_tarr_OKflag=0;
      sw_timer_start(&servo_y.timer, 1, 0);//1S超时
      break;
    }
    return 1;
}
/**
 * 获取转矩配置是否成功
 * 
 * @author laz
 * 
 * @param axis 
 */
int get_tarr_set(unsigned short axis)
{
   switch(axis)
  {
  case X_AXIS: 
    if(servo_x.set_tarr_OKflag==2)return 2;//配置成功
    if(sw_timer_expire(&servo_x.timer))return 0;//配置超时
    else return 1;//配置中  
    break;
  case Y_AXIS:
    if(servo_y.set_tarr_OKflag==2)return 2;//配置成功
    if(sw_timer_expire(&servo_y.timer))return 0;//配置超时
    else return 1;//配置中  
    break;
  }
  return 0;
}
//设置转矩模式下的X配置初始化
static int set_ctrl_tarr_axis_init(unsigned short axis,servo_param_t *servo_axis,unsigned short mode)
{
  switch(mode)
  {
  case 0:
    servo_axis->tarr_source_sel=0;//来源A
    servo_axis->tarr_modea_sel=0;
    servo_axis->tarr_modeb_sel=0;
    servo_axis->tarr_limit_sel=0;
    servo_axis->tarr_ai_sel=1;
    servo_axis->tarr_Plimit1_sel=3000;
    servo_axis->tarr_Nlimit1_sel=3000;
    servo_axis->tarr_Plimit2_sel=3000;
    servo_axis->tarr_Nlimit2_sel=3000;
    servo_axis->speed_limit_sel=0;
    servo_axis->speed_ai_sel=1;
    servo_axis->speed_plimit_set=200;
    servo_axis->speed_nlimit_set=100;
    servo_axis->tarr_limit_set=50; 
    servo_com_cmd(axis,TARR_SOURCE_SEL);    
    //转矩的基准值设置
    servo_axis->tarr_basis=0;
    servo_axis->tarr_basis_up=200;
    servo_axis->tarr_basis_dw=0;
    servo_com_cmd(axis,TARR_BASE_SET);
    //servo_axis->cmd_fb[TARR_SOURCE_SEL]=0;//清除反馈命令   
    break;
  case 1:
    servo_axis->tarr_source_sel=3;//来源A
    servo_axis->tarr_modea_sel=0;
    servo_axis->tarr_modeb_sel=0;
    servo_axis->tarr_limit_sel=2;
    servo_axis->tarr_ai_sel=1;
    servo_axis->tarr_Plimit1_sel=200;
    servo_axis->tarr_Nlimit1_sel=200;
    servo_axis->tarr_Plimit2_sel=1000;
    servo_axis->tarr_Nlimit2_sel=1000;
    servo_axis->speed_limit_sel=0;
    servo_axis->speed_ai_sel=1;
    servo_axis->speed_plimit_set=200;
    servo_axis->speed_nlimit_set=100;
    servo_axis->tarr_limit_set=1000; 
    servo_com_cmd(axis,TARR_SOURCE_SEL);
    //转矩的基准值设置
    servo_axis->tarr_basis=0;
    servo_axis->tarr_basis_up=200;
    servo_axis->tarr_basis_dw=0;
    servo_com_cmd(axis,TARR_BASE_SET);
    //servo_axis->cmd_fb[TARR_SOURCE_SEL]=0;//清除反馈命令   
    break;
  }
  return 1;
}
//设置转矩模式下的配置
static int set_ctrl_tarr_config(unsigned short axis,unsigned short mode)
{
  switch(axis)
  {
  case X_AXIS:        
    set_ctrl_tarr_axis_init(axis,&servo_x,mode);
    break;
  case Y_AXIS:
    set_ctrl_tarr_axis_init(axis,&servo_y,mode);
    break;
  }
  return 1;
}
//设置伺服运行模式
//0位置+转矩模式，转矩改变由通讯方式
//1转矩模式，采用转矩切换方式
static void set_servo_axis_mode(unsigned short axis,servo_param_t *servo_axis,unsigned short mode)
{  
  //SetEn(axis, MOTOR_EN);//停机关使能
  switch(mode)
  {
  case 0:
    //配置DI5功能为模式1切换和极性
    servo_axis->di_function_set=DI__FUNCTION_POSTARRSEL;
    servo_axis->di_polarity= DI_NCLOSE;
    servo_com_cmd(axis,DI_MODE_SW);
    //servo_axis->cmd_fb[DI_MODE_SW]=0;//清除反馈命令
    //配置DO3为转矩到达输出和极性
    servo_axis->do_function_set=DO_FUNCTION_VALUE;
    servo_axis->do_polarity= DI_NCLOSE;
    servo_com_cmd(axis,DO_TARR_SW);
    //servo_axis->cmd_fb[DO_TARR_SW]=0;//清除反馈命令
    //配置控制模式
    servo_axis->ctrl_mode_set=CTRL_POSTOTARR_MODE;
    servo_com_cmd(axis,CTRL_MODE_SET);
    //servo_axis->cmd_fb[CTRL_MODE_SET]=0;//清除反馈命令
    //配置转矩的方式：
    set_ctrl_tarr_config(axis,mode);
    //配置扭矩模式下的速度限制、扭矩限制、扭矩基准值
    //set_servo_postotarr_limit(axis,550,10000,10000,SERVO_TARR_CCW);
    //配置超时设置
    sw_timer_start(&servo_axis->timer, 2, 0);//2S超时
   break;
  case 1:
    //配置DI5功能为模式1切换和极性
    servo_axis->di_function_set=DI__FUNCTION_TARRSEL;
    servo_axis->di_polarity= DI_NOPEN;
    servo_com_cmd(axis,DI_MODE_SW);
    //servo_axis->cmd_fb[DI_MODE_SW]=0;//清除反馈命令
    //配置DO3为转矩到达输出和极性
    servo_axis->do_function_set=DO_FUNCTION_VALUE;
    servo_axis->do_polarity= DI_NCLOSE;
    servo_com_cmd(axis,DO_TARR_SW);
    //servo_axis->cmd_fb[DO_TARR_SW]=0;//清除反馈命令
    //配置控制模式
    servo_axis->ctrl_mode_set=CTRL_TARR_MODE;
    servo_com_cmd(axis,CTRL_MODE_SET);
    //servo_axis->cmd_fb[CTRL_MODE_SET]=0;//清除反馈命令
    //配置转矩的方式：
    set_ctrl_tarr_config(axis,mode);
    //配置扭矩模式下的速度限制、扭矩限制、扭矩基准值
    //set_servo_tarr_limit(axis,1000,1000,20000,10000,SERVO_TARR_CCW);
    //配置超时设置
    sw_timer_start(&servo_axis->timer, 2, 0);//2S超时
    break;
  }
}
//设置伺服运行模式
//0位置+转矩模式，转矩改变由通讯方式
//1转矩模式，采用转矩切换方式
static void set_servo_mode(unsigned short axis,unsigned short mode)
{
  switch(axis)
  {
  case X_AXIS:        
    set_servo_axis_mode(X_AXIS,&servo_x,mode);
    break;
  case Y_AXIS:
    set_servo_axis_mode(Y_AXIS,&servo_y,mode);
    break;
  }
}

/**
 * 伺服通讯初初始化函数
 * 
 * @author laz
 * 
 * @param cmd 
  */
static int servo_com_init(unsigned short axis,servo_param_t *servo_axis)
{
  //通讯握手三次
  switch(servo_axis->init_step)
  {
  case 0://握手
    servo_axis->IO_TO_COM=0;
    servo_com_cmd(axis,HANDSHAKE);
    //servo_axis->cmd_fb[HANDSHAKE]=0;//清除反馈命令
    sw_timer_start(&servo_axis->timer, 1, 0);//1S超时
    servo_axis->init_step++;
    servo_axis->com_number++;  
    break;
  case 1://判断握手情况。
    if (sw_timer_expire(&servo_axis->timer)){//握手超时
        if(servo_axis->com_number>=3){//三次通讯超时，通讯失败。
          servo_axis->init_step=0xF0;          
        }
        else{//没有到三次继续通讯。
          servo_axis->init_step=0;
        }
    }
    if(servo_axis->cmd_fb[HANDSHAKE]==HANDSHAKE)//通讯成功
    {
      servo_axis->com_ok_number++;
      if(servo_axis->com_ok_number>=3){//通讯成功3次
        servo_axis->init_step++;
        servo_axis->com_number=0;
      }
      else{
        servo_axis->init_step=0;
      }
    }
    break;
  case 2://配置伺服驱动器。
     //停机关使能
    //SetEn(axis, 1);
    set_servo_mode(axis,RUN_MODE);
    servo_axis->com_number++;
    sw_timer_start(&servo_axis->timer, 2, 0);//1S超时
    servo_axis->init_step++;
    break; 
  case 3://判断配置IO情况。
    if (sw_timer_expire(&servo_axis->timer)){//配置失败
        if(servo_axis->com_number>=3){//三次配置失败，通讯失败。
          servo_axis->init_step=0xF1;          
        }
        else{//配置没有成功重新配置。
          servo_axis->init_step=2;
        }         
    }
    //配置成功
    if(servo_axis->cmd_fb[DI_MODE_SW]==DI_MODE_SW&&servo_axis->cmd_fb[DO_TARR_SW]==DO_TARR_SW
       &&servo_axis->cmd_fb[CTRL_MODE_SET]==CTRL_MODE_SET
       &&servo_axis->cmd_fb[TARR_SOURCE_SEL]==TARR_SOURCE_SEL 
       //&&(servo_axis->cmd_fb[SPEED_LIMIT_SET]==SPEED_LIMIT_SET&&RUN_MODE==0
       //||servo_axis->cmd_fb[TARR_LIMIT12_SET]==TARR_LIMIT12_SET&&RUN_MODE==1)
       &&servo_axis->cmd_fb[TARR_BASE_SET]==TARR_BASE_SET){    
      servo_axis->init_step++;
    }
    break;
  case 4://初始化配置扭矩模式下的速度限制、扭矩限制、扭矩基准值
    if(RUN_MODE==0)
    {
      set_servo_postotarr_limit(axis,550,10000,10000,SERVO_TARR_CCW);
    }
    else
    {
      set_servo_tarr_limit(axis,1000,1000,20000,10000,SERVO_TARR_CCW);
    }
    servo_axis->init_step++;
    break;
  case 5://设置告警和清除告警的读取模式，循环读取ALARMCODE；
    Set_Servo_Runmode(axis,1);//设置伺服运行在通讯模式，
    servo_com_cmd(axis,ALARM_CODE);
    //servo_axis->cmd_fb[ALARM_CODE]=0;//清除反馈命令
    servo_axis->init_step++;
    break;
  case 6://配置成功，输出使能
    servo_axis->IO_TO_COM=1;//配置为IO模式
    servo_axis->init_step=101;
    servo_com_cmd(axis,ENCODE_FB);
    return 1;
    break;
  case 0xF1://配置失败处理
    servo_com_cmd(axis,SERVO_RESTART);//伺服重启
    servo_axis->cmd_fb[SERVO_RESTART]=0;//清除反馈命令
    servo_axis->init_step++;
    break;
  case 0xF2://配置失败处理    
    if(servo_axis->cmd_fb[SERVO_RESTART]==SERVO_RESTART){
      ;//SetEn(X_AXIS, MOTOR_EN);//设置使能
      return 0xF2;
    }
  case 0xF0://配置失败处理
    return 0xF0;
    break;
  }
  return 0;
}
//获取伺服通讯初始化状态
int GetServoComState(unsigned short axis)
{
  switch(axis)
  {
  case X_AXIS:
    return servo_x.IO_TO_COM;
    break;
  case Y_AXIS:
    return servo_x.IO_TO_COM;
    break;
  }
  return 0;
}
//设置扭矩的应用
void SetServoComUse(unsigned short axis,unsigned short enable)
{
   switch(axis)
  {
  case X_AXIS:
    if(enable)
    { 
      SetEn(X_AXIS, MOTOR_DISEN);
      servo_x.init_step=0;
    }
    else
    {
      servo_x.init_step=0xF0;
    }
    break;
  case Y_AXIS:
    if(enable)
    { 
      SetEn(Y_AXIS, MOTOR_DISEN);
      servo_y.init_step=0;
    }
    else
    {
      servo_y.init_step=0xF0;
    }
    break;
  }
}
//伺服运行测试位置
static sw_timer_t servo_com_timer;//伺服通讯定时器。
unsigned char servo_test_step=0;
long X_pos;
int servo_test_pluse_mode(void)
{  
  static long servo_test_delay=0;
  memcpy(&user_datas[610],&dwYRealPos,4);
  memcpy(&user_datas[612],&olddwYRealPos[0],4);
  SERVO_STEP=servo_test_step;
  switch(servo_test_step)
  {
  case 1:
    if(RUN_MODE==0)servo_test_step++;//转矩模式+位置模式测试
    else servo_test_step=20;//转矩模式的，转矩1，2限制测试
    break;
  case 2:
    //SetEn(X_AXIS, MOTOR_EN);
    SetEn(Y_AXIS, MOTOR_EN);
    //SetPos(X_AXIS,0); 
    SetPos(Y_AXIS,0);    
    //扭矩模式+位置模式(轴--扭矩%--正转速度限制--反转速度限制--方向)
    //set_servo_postotarr_limit(X_AXIS,200,400,100,SERVO_TARR_CW);
    set_servo_postotarr_limit(Y_AXIS,200,4000,2000,SERVO_TARR_CW);
    //设置位置模式
    Set_Ctrlmode_trans(Y_AXIS,POS_MODE);
    //
    //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE_LIMIT1); 
    sw_timer_start(&servo_com_timer, 10, 0);
    servo_test_step++;
    //CTHL_XSavePosBuff=servo_com_timer.usec;
    break;
  case 4://位置运行
    AxisMovePosAccDecNotStop(Y_AXIS,2000,50000,1000,1000,20,50,50);
    servo_test_step++;
    break;
  case 5:
    if(get_tarr_set(Y_AXIS)==2)//设置扭矩完成
    {
      if(abs(dwYRealPos)>=10000)
      {        
        Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);//设置成为扭矩模式
        AxisEgmStop(Y_AXIS);
        servo_test_step++;
      }
    }
    else if(get_tarr_set(Y_AXIS)==0)//配置超时
    {
       servo_test_step=0xF0;
    }
    break;  
  case 6://
    if(GetTarr(Y_AXIS))//转矩到达
    {
       servo_test_delay=dwTickCount + 1000;  //维持1S    
       servo_test_step++;
    }
    break;
  case 7://
   if(dwTickCount>=servo_test_delay)
   {
      if(GetTarr(Y_AXIS)) 
      {
        Set_Ctrlmode_trans(Y_AXIS,POS_MODE);//转位置模式;
        set_servo_postotarr_limit(Y_AXIS,250,4000,2000,SERVO_TARR_CCW);//配置转矩及方向
        servo_test_step++;
      }
   }
    break; 
  case 8://松轴
     AxisMovePosAccDecNotStop(Y_AXIS,2000,-50000,1000,1000,20,50,50);
     servo_test_step++;
    break; 
  case 9:
    if(get_tarr_set(Y_AXIS)==2)//设置扭矩完成
    {
      if(abs(dwYRealPos)>=10000)
      {        
        Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);//设置成为扭矩模式  
        servo_test_delay=dwTickCount + 5000;
        servo_test_step++;
      }
    }
    else if(get_tarr_set(Y_AXIS)==0)//配置超时
    {
       servo_test_step=0xF0;
    }
    break; 
  case 10://
    if(dwTickCount>=servo_test_delay)
    {
      if(GetTarr(Y_AXIS))//转矩到达
      {
         servo_test_delay=dwTickCount + 1000;  //维持1S    
         servo_test_step++;
      }
    }
    break;
  case 11://
   if(dwTickCount>=servo_test_delay)
   {
      if(GetTarr(Y_AXIS)) 
      {
        Set_Ctrlmode_trans(Y_AXIS,POS_MODE);//转位置模式; 
        servo_test_delay=dwTickCount + 100;
        servo_test_step++;
      }
   }
   break;
  case 12://1S后转转矩模式
   if(dwTickCount>=servo_test_delay)
   {
      Set_Ctrlmode_trans(Y_AXIS,TARR_MODE);//转位置模式; 
      servo_test_step++;
   }
   break;
  case 13://
    if(GetTarr(Y_AXIS))//转矩到达
    {
       servo_test_delay=dwTickCount + 1000;  //维持1S   
       AxisEgmStop(Y_AXIS);
       servo_test_step++;
    }
    break;
  case 14://
   if(dwTickCount>=servo_test_delay)
   {
      if(GetTarr(Y_AXIS)) 
      {
        Set_Ctrlmode_trans(Y_AXIS,POS_MODE);//转位置模式; 
        servo_test_step=0;
      }
   }
   break;
  case 20://配置扭矩
    //SetEn(X_AXIS, MOTOR_EN);
    
    //SetPos(X_AXIS,0); 
    SetPos(Y_AXIS,0);    
    //扭矩模式，用扭矩1和扭矩2之间的切换
    //扭矩模式+位置模式(轴--最大扭矩--最小扭矩--正转速度限制--反转速度限制--方向)
    //set_servo_tarr_limit(X_AXIS,200,100,400,100,SERVO_TARR_CW);
    set_servo_tarr_limit(Y_AXIS,200,100,4000,2000,SERVO_TARR_CW);
    //设置最大扭矩限制
    Set_Ctrlmode_trans(Y_AXIS,TARR_MODE_MAXLIMIT);
    //
    //Set_Ctrlmode_trans(Y_AXIS,TARR_MODE_LIMIT1); 
    sw_timer_start(&servo_com_timer, 10, 0);
    //servo_test_step++;
   break;
   case 21://扭矩运行
    if(get_tarr_set(Y_AXIS)==2)//设置扭矩完成
    {
      if(abs(dwYRealPos)>=10000)
      {        
        SetEn(Y_AXIS, MOTOR_EN);//运行
        servo_test_step++;
      }
    }
    else if(get_tarr_set(Y_AXIS)==0)//配置超时
    {
       servo_test_step=0xF0;
    }
   break;
  case 22://维持运行
     servo_test_delay=dwTickCount + 10000;  //维持10S   
     servo_test_step++;
    break;
   case 23://切换扭矩，并维持运行
   if(dwTickCount>=servo_test_delay)
   {
      Set_Ctrlmode_trans(Y_AXIS,TARR_MODE_MINLIMIT); //切换成扭矩   
       servo_test_delay=dwTickCount + 10000;  //维持10S  
      servo_test_step++;      
   }
   break;
   case 24://停止，切换方向，重新设置
   if(dwTickCount>=servo_test_delay)
   {
      SetEn(Y_AXIS, MOTOR_EN);//停止
      set_servo_tarr_limit(Y_AXIS,200,100,4000,2000,SERVO_TARR_CCW);//切换方向
      Set_Ctrlmode_trans(Y_AXIS,TARR_MODE_MAXLIMIT);
      servo_test_step++;      
   }
   break;
   case 25://运行
    if(get_tarr_set(Y_AXIS)==2)//设置扭矩完成
    {
      if(abs(dwYRealPos)>=10000)
      {        
        SetEn(Y_AXIS, MOTOR_EN);//运行
        servo_test_step++;
      }
    }
    else if(get_tarr_set(Y_AXIS)==0)//配置超时
    {
       servo_test_step=0xF0;
    }    
   break;
   case 26://维持运行
     servo_test_delay=dwTickCount + 10000;  //维持10S   
     servo_test_step++;
    break;
   case 27://切换扭矩
   if(dwTickCount>=servo_test_delay)
   {
      Set_Ctrlmode_trans(Y_AXIS,TARR_MODE_MINLIMIT); //切换成扭矩   
       servo_test_delay=dwTickCount + 10000;  //维持10S  
      servo_test_step++;      
   }
   break;
   case 28://停止
   if(dwTickCount>=servo_test_delay)
   {
      SetEn(Y_AXIS, MOTOR_EN);//停止
      servo_test_step=0;      
   }
   break;
  }
  if(M0045)
  {
    M0045=0;
    AxisContinueMoveChangeSpeed(Y_AXIS,CHANGE_SPEED,STOP_SPEED,10,10);
  }
  if(M0046)
  {
    M0046=0;
    servo_test_step++;
  }
  if(M0047)
  {
    M0047=0;
    //配置扭矩模式下的速度限制。
    servo_y.speed_plimit_set=LIMIT_SPEED;
    servo_y.speed_nlimit_set=LIMIT_SPEED;
    servo_com_cmd(Y_AXIS,SPEED_LIMIT_SET);
    servo_y.cmd_fb[SPEED_LIMIT_SET]=0;//清除反馈命令
  }
  return 0;
}
//伺服运行
int servo_com_run(void)
{
  int runflag;
#if  X_USERING_TARR==1
  runflag=servo_com_init(X_AXIS,&servo_x);//初始化
  servo_com_get_alarm(X_AXIS);
#endif
#if  Y_USERING_TARR==1
  runflag=servo_com_init(Y_AXIS,&servo_y);//初始化
  servo_com_get_alarm(Y_AXIS);
#endif
  //servo_test_pluse_mode();
  return runflag;
}