#define yqy_dir		PTA_PTA5	//右氣壓表方向位	1-反轉(zhuǎn)   0-正轉(zhuǎn)
#define zqy_dir		PTA_PTA3	//左氣壓表		  	1-反轉(zhuǎn)   0-正轉(zhuǎn)
#define cs_dir		PTA_PTA1	//車速表      		1-反轉(zhuǎn)   0-正轉(zhuǎn)
#define zs_dir		PTA_PTA7	//轉(zhuǎn)速表 		      1-反轉(zhuǎn)   0-正轉(zhuǎn)
#define dy_dir    PTF_PTF1  //電壓表 		      1-反轉(zhuǎn)   0-正轉(zhuǎn)
#define ry_dir		PTF_PTF3	//燃油表 		      1-反轉(zhuǎn)   0-正轉(zhuǎn)
#define sw_dir		PTF_PTF7	//水溫表 		      1-反轉(zhuǎn)   0-正轉(zhuǎn)
#define jy_dir		PTF_PTF5	//機(jī)油壓力表	    1-反轉(zhuǎn)   0-正轉(zhuǎn)

#define yqy_sig		PTA_PTA4		//右氣壓表		0 -> 1 驅(qū)動
#define zqy_sig		PTA_PTA2		//左氣壓表		0 -> 1 驅(qū)動
#define cs_sig		PTA_PTA0		//車速表	  	0 -> 1 驅(qū)動						
#define zs_sig		PTA_PTA6		//轉(zhuǎn)速表 	  	0 -> 1 驅(qū)動.
#define dy_sig		PTF_PTF0		//電壓表	  	0 -> 1 驅(qū)動
#define ry_sig		PTF_PTF2		//燃油表	  	0 -> 1 驅(qū)動
#define sw_sig		PTF_PTF6		//水溫表	  	0 -> 1 驅(qū)動						
#define jy_sig		PTF_PTF4		//機(jī)油壓力表 	0 -> 1 驅(qū)動

//#define	RST_6606	PTC_PTC6		//6606復(fù)位引腳
//以上端口定義

sys_data_type		_motor_flags0;

#define		cs_dir_flag		_motor_flags0.Bit.BIT0    //正反轉(zhuǎn)方向標(biāo)志位
#define		zs_dir_flag		_motor_flags0.Bit.BIT1
#define		yqy_dir_flag	_motor_flags0.Bit.BIT2
#define		zqy_dir_flag	_motor_flags0.Bit.BIT3
#define		dy_dir_flag		_motor_flags0.Bit.BIT4
#define		sw_dir_flag		_motor_flags0.Bit.BIT5
#define		ry_dir_flag		_motor_flags0.Bit.BIT6
#define		jy_dir_flag		_motor_flags0.Bit.BIT7

//以上標(biāo)志位

//
unsigned char   yqy_fcan;  //CAN_ZKC4.Byte
unsigned char   zqy_fcan;  //CAN_ZKC5.Byte 
unsigned char   cs_fcan;  //CAN_WKD2.Byte 
unsigned char   zs_fcanh;  //CAN_WKD0.Byte
unsigned char   zs_fcanl;  //CAN_WKD1.Byte
unsigned char   dy_temp;   //CAN_YB6.Byte
unsigned char   ry_fcan;   //CAN_ZKC6.Byte
unsigned char   sw_fcan;   //CAN_WKC5.Byte
unsigned char   jy_fcan;		//CAN_WKC4.Byte
//參數(shù)接口定義

//右氣壓
unsigned char yqy_last=0;         //上一次數(shù)據(jù)
unsigned int  yqy_stepall=0;      //實(shí)時總步數(shù)
unsigned int  yqy_step_change=0;  //要走的總步數(shù) 
unsigned int  yqy_step_diff;      //步數(shù)差
unsigned char yqy_dec_speed;			//指針?biāo)俣葏?shù)
unsigned char yqy_dec_count=0;		//速度變量
//左氣壓
unsigned char zqy_last=0;
unsigned int  zqy_stepall=0;
unsigned int  zqy_step_change=0;
unsigned int  zqy_step_diff;
unsigned char zqy_dec_speed;
unsigned char zqy_dec_count=0;
//車速
unsigned char cs_last=0;
unsigned int  cs_stepall=0;
unsigned int  cs_step_change=0;
unsigned int  cs_step_diff;
unsigned char cs_dec_speed;
unsigned char cs_dec_count=0;
//轉(zhuǎn)速--16位
unsigned int  zs_last=0;
unsigned int  zs_stepall=0;
unsigned int  zs_step_change=0;
unsigned int  zs_step_diff;
unsigned char zs_dec_speed;
unsigned char zs_dec_count=0;
//系統(tǒng)電壓
unsigned char dy_last=18;         //系統(tǒng)電壓顯示最小值
unsigned int  dy_stepall=0;
unsigned int  dy_step_change=0;
unsigned int  dy_step_diff;
unsigned char dy_dec_speed;
unsigned char dy_dec_count=0;
//燃油
unsigned char ry_last=0;
unsigned int  ry_stepall=0;
unsigned int  ry_step_change=0;
unsigned int  ry_step_diff;
unsigned char ry_dec_speed;
unsigned char ry_dec_count=0;
//水溫
unsigned char sw_last=40;         //水溫顯示最小值
unsigned int  sw_stepall=0;
unsigned int  sw_step_change=0;
unsigned int  sw_step_diff;
unsigned char sw_dec_speed;
unsigned char sw_dec_count=0;
//機(jī)油
unsigned char jy_last=0;
unsigned int  jy_stepall=0;
unsigned int  jy_step_change=0;
unsigned int  jy_step_diff;
unsigned char jy_dec_speed;
unsigned char jy_dec_count=0;
/////

void yb_step_first(void){
  zqy_fcan=CAN_ZKC4.Byte;
  yqy_fcan=CAN_ZKC5.Byte;//氣壓左右交換過 
  cs_fcan=CAN_WKD2.Byte; 
  zs_fcanh=CAN_WKD0.Byte;
  zs_fcanl=CAN_WKD1.Byte;
  dy_temp=CAN_YB6.Byte;
  ry_fcan=CAN_ZKC6.Byte;
  sw_fcan=CAN_WKC5.Byte;
  jy_fcan=CAN_WKC4.Byte;
}
//*******************************************************************************
//函數(shù): void motor_go0(void)
//描述: 步進(jìn)電機(jī)回零函數(shù)
//參數(shù): none
//返回值:none
//*******************************************************************************
void motor_go0(void){//一脈沖1/12度
    unsigned int i;
    comp_motor_flag=0;
    PTF=0;//寫
    yqy_dir=1;		  
    zqy_dir=1;		  
    cs_dir=1;	  
    zs_dir=1;		
    dy_dir=1;	
    ry_dir=1;	
    sw_dir=1;	
    jy_dir=1; //反轉(zhuǎn)
    
    for(i=0;i<100;i++)
    {//走100步
      yqy_sig=0;
      zqy_sig=0;	
      cs_sig=0;						
      zs_sig=0;	
      dy_sig=0;	
      ry_sig=0;	
      sw_sig=0;						
      jy_sig=0;
      delay(150);
      __RESET_WATCHDOG();		//喂狗
      yqy_sig=1;
      zqy_sig=1;	
      cs_sig=1;						
      zs_sig=1;	
      dy_sig=1;	
      ry_sig=1;	
      sw_sig=1;						
      jy_sig=1;
    }
    
    for(i=0;i<3700;i++)
    {//走3700步
      yqy_sig=0;
      zqy_sig=0;	
      cs_sig=0;						
      zs_sig=0;	
      dy_sig=0;	
      ry_sig=0;	
      sw_sig=0;						
      jy_sig=0;
      delay(50);
      __RESET_WATCHDOG();		//喂狗
      yqy_sig=1;
      zqy_sig=1;	
      cs_sig=1;						
      zs_sig=1;	
      dy_sig=1;	
      ry_sig=1;	
      sw_sig=1;						
      jy_sig=1;
    }
    
        
    yqy_last=0;yqy_step_change=0;yqy_stepall=0;
    zqy_last=0;zqy_step_change=0;zqy_stepall=0;
    cs_last=0;cs_step_change=0;cs_stepall=0;
    zs_last=0;zs_step_change=0;zs_stepall=0;
    dy_last=18;dy_step_change=0;dy_stepall=0;
    ry_last=0;ry_step_change=0;ry_stepall=0;
    sw_last=40;sw_step_change=0;sw_stepall=0;
    jy_last=0;jy_step_change=0;jy_stepall=0;
    
    CAN_ZKC4.Byte=0;
    CAN_ZKC5.Byte=0; 
    CAN_WKD2.Byte=0; 
    CAN_WKD0.Byte=0;
    CAN_WKD1.Byte=0;
    CAN_YB6.Byte=18;
    CAN_ZKC6.Byte=0;
    CAN_WKC5.Byte=40;
    CAN_WKC4.Byte=0;			 //未收到CAN數(shù)據(jù)前
    comp_motor_flag=1;
}

//******************************
//函數(shù): void yqy_step_count(void)
//描述: 右氣壓步數(shù)計算
//參數(shù): none
//返回值:none
//******************************
void yqy_step_count(void){
    unsigned int yb_temp;
    unsigned char yqy_speed_temp;
    if(yqy_fcan>94) yqy_fcan=94;
    
    if(yqy_fcan!=yqy_last){
      yqy_last=yqy_fcan;
      
      yb_temp=yqy_fcan;
      yb_temp=yb_temp*33;		                      //計算轉(zhuǎn)動步數(shù)---33.024
      
      if(yb_temp<yqy_stepall){                    //反
        yqy_step_diff=yqy_stepall-yb_temp;
        yqy_dir_flag=1;
      }
      else{                                       //正
        yqy_step_diff=yb_temp-yqy_stepall;
        yqy_dir_flag=0;
      }      
      yqy_step_change=yb_temp;
    }
    //
    if(yqy_step_diff>750)   yqy_speed_temp=1;
    else                    yqy_speed_temp=(unsigned char)(1500/yqy_step_diff);
    if(yqy_step_diff<30)    yqy_speed_temp=50;
    yqy_dec_speed=yqy_speed_temp;
}
//******************************
//函數(shù): void yqy_step_oper(void)
//描述: 右氣壓操作
//參數(shù): none
//返回值:none
//******************************
void yqy_step_oper(void){
  //unsigned char yb_temp0;
  //if(yqy_step_change!=yqy_stepall){
    yqy_dec_count++;
    /*if(yqy_step_diff>36){yqy_dec_speed=1;}    //3度 
    else{                                     //計算速度參數(shù)
      yb_temp0=(unsigned char)(yqy_step_diff>>2);
      yqy_dec_speed=10-yb_temp0;
    }*/
    if(yqy_dec_count==yqy_dec_speed){
      yqy_step_diff--;
      yqy_dir=yqy_dir_flag;//1=反轉(zhuǎn)
      yqy_dec_count=0;
      yqy_sig=0;
      if(yqy_dir_flag){yqy_stepall--;}			  //反轉(zhuǎn)
      else{yqy_stepall++;}										//正轉(zhuǎn)
      yqy_sig=1;  
    }
  //}
}

//******************************
//函數(shù): void zqy_step_count(void)
//描述: 左氣壓步數(shù)計算
//參數(shù): none
//返回值:none
//******************************
void zqy_step_count(void)
{  
    unsigned int yb_temp;
    unsigned char zqy_speed_temp;
    if(zqy_fcan>94) zqy_fcan=94;
    
    if(zqy_fcan!=zqy_last){
      zqy_last=zqy_fcan;
      
      yb_temp=zqy_fcan;
      yb_temp=yb_temp*33;		                      //計算轉(zhuǎn)動步數(shù)---33.024
      
      if(yb_temp<zqy_stepall){                    //反
        zqy_step_diff=zqy_stepall-yb_temp;
        zqy_dir_flag=1;
      }
      else{                                       //正
        zqy_step_diff=yb_temp-zqy_stepall;
        zqy_dir_flag=0;
      }      
      zqy_step_change=yb_temp;
    }
    //
    if(zqy_step_diff>750)   zqy_speed_temp=1;
    else                    zqy_speed_temp=(unsigned char)(1500/zqy_step_diff);
    if(zqy_step_diff<30)    zqy_speed_temp=50;
    zqy_dec_speed=zqy_speed_temp;  
}
//******************************
//函數(shù): void zqy_step_oper(void)
//描述: 左氣壓操作
//參數(shù): none
//返回值:none
//******************************
void zqy_step_oper(void){  
  //unsigned char yb_temp0;
  //if(zqy_step_change!=zqy_stepall){
    zqy_dec_count++;
    /*if(zqy_step_diff>36){zqy_dec_speed=1;}    //3度 
    else{                                     //計算速度參數(shù)
      yb_temp0=(unsigned char)(zqy_step_diff>>2);
      zqy_dec_speed=10-yb_temp0;
    }*/
    if(zqy_dec_count==zqy_dec_speed){
      zqy_step_diff--;
      zqy_dir=zqy_dir_flag;//1=反轉(zhuǎn)
      zqy_dec_count=0;
      zqy_sig=0;
      if(zqy_dir_flag){zqy_stepall--;}			  //反轉(zhuǎn)
      else{zqy_stepall++;}										//正轉(zhuǎn)
      zqy_sig=1;  
    }
  //}
}

//******************************
//函數(shù): void cs_step_count(void)
//描述: 車速步數(shù)計算
//參數(shù): none
//返回值:none
//******************************
void cs_step_count(void)
{  
    unsigned int yb_temp;
    unsigned char cs_speed_temp;
    if(cs_fcan>162) cs_fcan=162;
    
    if(cs_fcan!=cs_last){
      cs_last=cs_fcan;
      
      yb_temp=cs_fcan;
      
      yb_temp=yb_temp*84/5;		                //計算轉(zhuǎn)動步數(shù)  12微步*14度/10（Km/h）
      /*160Km/h<->224度   1Km/h<->224*12/160*/
      if(yb_temp>17)  yb_temp+=36;
      if(yb_temp<cs_stepall){                    //反
        cs_step_diff=cs_stepall-yb_temp;
        cs_dir_flag=1;
      }
      else{                                       //正
        cs_step_diff=yb_temp-cs_stepall;
        cs_dir_flag=0;
      }      
      cs_step_change=yb_temp;
    }
    //
    if(cs_step_diff>400)    cs_speed_temp=1;