ALPS-004/WZYXHT_V1.12507260002.X/WZYXHT_V1.12507260002.X/Motor.c

#include "MOTOR.h"
#include "delay.h"
#include "DS18B20.h"
#include "Instrument_EEPROM_Param.h"

extern unsigned char Motor_Status;

unsigned char         g_ucMotorXFinishFlag,g_ucMotorYFinishFlag,g_ucMotorZFinishFlag,g_ucMotor4FinishFlag,g_ucMotorZ2FinishFlag  ;   
unsigned int          g_uiAccIndex ;             
unsigned long int     g_uliRunningPulse ;       
unsigned long int     g_uliPulseCount ;           
unsigned int          g_uiReagentMotorPeriod[REAGENT_MOTOR_ACC],g_uiReagentMotorPeriod2[REAGENT_MOTOR_ACC2],g_uiReagentMotorPeriod3[REAGENT_MOTOR_ACC3],g_uiReagentMotorPeriod4[REAGENT_MOTOR_ACC4];
unsigned long         PulseNum;
unsigned char         Motor1_HOME_Flag,Motor2_HOME_Flag,Motor3_HOME_Flag,Motor4_HOME_Flag,Motor5_HOME_Flag;
unsigned int          Moving_Speed;   
unsigned int          Reset_Speed;    
struct             MotorParam            g_tReagentMotorParam;
struct             MotorParamEEPROM      Motor1;  
struct             MotorParamEEPROM      Motor2;
struct             MotorParamEEPROM      Motor3;
struct             MotorParamEEPROM      Motor4;
//struct             MotorParamEEPROM      Moor5;
void CalculateSModelLine( unsigned int * period, unsigned int len, unsigned int fre_max, unsigned int fre_min, float flexible)
{
	int i,ttt;
	float deno ;
	float melo ;
	float fre;
	float Fdelt = fre_max-fre_min;
	if(len>ACCLTH) len=ACCLTH;
	for(i=0; i<len; i++)
          {
		melo = flexible * (2.0*i/len-1) ;
		deno = 1.0 / (1 + expf(-melo));  
		fre = Fdelt * deno + fre_min;
		ttt=period[i] = (unsigned short)(625000.0/fre);
	}
}
void InitMotor1PWM(void)     
{
    PMCONbits.PTBEEN=0;         
    _RP25R = 0x12;                        
    OC1CON = 0;
    OC1CON = 0;
    OC1CONbits.OCM = 0;
    OC1CONbits.OCTSEL = 0x00; 

    OC1R = 312; 

    OC1RS =625;
    T2CON = 0;
    T2CONbits.TCKPS = 2;
    PR2 = 625; 
    OC1CONbits.OCM = 4;  
    T2CONbits.TON = 0;
    IEC0bits.T2IE = 0;           
    IFS0bits.T2IF = 0;
    IPC0bits.OC1IP = 6;
    IEC0bits.OC1IE = 0;
    IFS0bits.OC1IF = 0;   
}

void InitMotor2PWM(void)     
{  
    PMCONbits.PTBEEN=0;          
    _RP4R = 0x13;              
    OC2CON=0;
    OC2CON = 0; 
    OC2CONbits.OCM = 0;
    OC2CONbits.OCTSEL = 0x00  ; 
    OC2R = 312; 
    OC2RS = 625;
    T2CON = 0;
    T2CONbits.TCKPS = 2;
    PR2 = 625;
    OC2CONbits.OCM = 4;
    T2CONbits.TON = 0; 
    IEC0bits.T2IE = 0;       
    IFS0bits.T2IF = 0;   
    IPC1bits.OC2IP = 6; 
    IEC0bits.OC2IE = 0;
    IFS0bits.OC2IF = 0;   
}

void InitMotor3PWM(void)      
{
    PMCONbits.PTBEEN=0;           
    _RP5R = 0x14;              
    OC3CON = 0;
    OC3CON = 0; 
    OC3CONbits.OCM = 0;
    OC3CONbits.OCTSEL = 0x00;
    OC3R = 312; 
    OC3RS = 625;
    T2CON = 0;
    T2CONbits.TCKPS = 2;
    PR2 = 625; 
    OC3CONbits.OCM = 4;
    T2CONbits.TON = 0; 
    IEC0bits.T2IE = 0;          
    IFS0bits.T2IF = 0;   
    IPC6bits.OC3IP = 6;	
    IEC1bits.OC3IE = 0;
    IFS1bits.OC3IF = 0;  
}

void InitMotor4PWM(void)    
{
    PMCONbits.PTBEEN=0;           
    _RP13R = 0x15;                
    OC4CON = 0;
    OC4CON = 0; 
    OC4CONbits.OCM = 0;
    OC4CONbits.OCTSEL = 0x00; 
    OC4R = 312; 
    OC4RS = 625;
    T2CON = 0;
    T2CONbits.TCKPS = 2;
    PR2 = 625; 
    OC4CONbits.OCM = 4; 
    T2CONbits.TON = 0;
    IEC0bits.T2IE = 0;            
    IFS0bits.T2IF = 0;   
    IPC6bits.OC4IP = 6;	
    IEC1bits.OC4IE = 0;
    IFS1bits.OC4IF = 0;  
}

void SetMotor1(unsigned long int Pulses,unsigned int spd,unsigned long int *AccPLS,unsigned long int *constVPLS)
{
	unsigned int i;
	unsigned int Prd;
	Prd=625000/spd;
            *AccPLS = 0;
	for(i=0;i<REAGENT_MOTOR_ACC;i++)
            {
		if((Prd < g_uiReagentMotorPeriod[i])&&(Prd >= g_uiReagentMotorPeriod[i+1]))
                        {
			*AccPLS=i+1;
			break;
		}
		else if(Prd == g_uiReagentMotorPeriod[i])
                        {
			*AccPLS=i;
			break;
		}
	}
	if(Pulses >= *AccPLS + *AccPLS)
            {
                         *constVPLS = Pulses- *AccPLS;
            }
	else if(Pulses < *AccPLS + *AccPLS) 
            {
		*AccPLS = Pulses/2;
		*constVPLS = *AccPLS;
	}	
}
void SetMotor2(unsigned long int Pulses,unsigned int spd,unsigned long int *AccPLS,unsigned long int *constVPLS)
{
	unsigned int i;
	unsigned int Prd;
	Prd=625000/spd;
          *AccPLS = 0;
	for(i=0;i<ACC_pulse2;i++)
         {
		if((Prd < g_uiReagentMotorPeriod2[i])&&(Prd >= g_uiReagentMotorPeriod2[i+1]))
                   {
                        *AccPLS=i+1;
                        break;
                    }
		else if(Prd == g_uiReagentMotorPeriod2[i])
                    {
                        *AccPLS=i;
                        break;
		}
	}
	if(Pulses >= (*AccPLS + *AccPLS))
         {
        *constVPLS = Pulses- *AccPLS;
          }
	else if(Pulses < (*AccPLS + *AccPLS)) 
          {
		*AccPLS = Pulses/2;
		*constVPLS = *AccPLS;
	}	
}
void SetMotor3(unsigned long int Pulses,unsigned int spd,unsigned long int *AccPLS,unsigned long int *constVPLS)
{
	unsigned int i;
	unsigned int Prd;
	Prd=625000/spd;
         *AccPLS = 0;
         if(spd>REAGENT_MOTOR_FREQMAX3||spd<REAGENT_MOTOR_FREQMIN3)
         {
                
         }
         else
         {
            for(i=0;i<REAGENT_MOTOR_ACC3;i++)
              {
                if((Prd < g_uiReagentMotorPeriod3[i])&&(Prd >= g_uiReagentMotorPeriod3[i+1]))
                    {
                        *AccPLS=i+1;
                        break;
                    }
                    else if(Prd == g_uiReagentMotorPeriod3[i])
                   {
                        *AccPLS=i;
                        break;
                    }
             }
	if(Pulses >= (*AccPLS + *AccPLS))
            {
                *constVPLS = Pulses- *AccPLS;
            }
	else if(Pulses < (*AccPLS + *AccPLS)) 
         {
		*AccPLS = Pulses/2;
        		*constVPLS = *AccPLS;
   	}
         }
}

void SetMotor4(unsigned long int Pulses,unsigned int spd,unsigned long int *AccPLS,unsigned long int *constVPLS)
{
	unsigned int i;
	unsigned int Prd;
	Prd=625000/spd;
         *AccPLS = 0;
         if(spd>REAGENT_MOTOR_FREQMAX4||spd<REAGENT_MOTOR_FREQMIN4)
         {
                
         }
         else
         {
            for(i=0;i<REAGENT_MOTOR_ACC4;i++)
              {
                if((Prd < g_uiReagentMotorPeriod4[i])&&(Prd >= g_uiReagentMotorPeriod4[i+1]))
                    {
                        *AccPLS=i+1;
                        break;
                    }
                    else if(Prd == g_uiReagentMotorPeriod4[i])
                   {
                        *AccPLS=i;
                        break;
                    }
             }
	if(Pulses >= (*AccPLS + *AccPLS))
            {
                *constVPLS = Pulses- *AccPLS;
            }
	else if(Pulses < (*AccPLS + *AccPLS)) 
         {
		*AccPLS = Pulses/2;
        		*constVPLS = *AccPLS;
   	}
         }
}

void Motor1Home(unsigned int pulseoffset,unsigned int MOVEspd,unsigned int LEAVspd )
{
    unsigned int Freq;
    if(g_tReagentMotorParam.ulZiPosition>Z_limitation1||g_tReagentMotorParam.ulZiPosition>Z1_limitation1)
    {
        if(X_move_protection_flag==0)
        {
            Motor_Status=13;
            return;
        }
    }
    Motor1_HOME_Flag=1;
    Motor2_HOME_Flag=0;
    Motor3_HOME_Flag=0;
    Motor4_HOME_Flag=0;
    Motor5_HOME_Flag=0;    
    OC1CON = 0;
    g_tReagentMotorParam.ucXDirection = MOTOR1_FORWARD;
    Delay1ms(2);
    TMR5=0;
    PulseNum=0;
    if(MOTOR1_HOME == 1)            
    {
        MOTOR1_DIR = MOTOR1_FORWARD;      
        Freq=5000000/LEAVspd;            
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(MOTOR1_HOME)
        {
            if(PulseNum > Motor1.Limit_Pulse*2){ Motor_Status=11;break;}
        }
        T5CONbits.TON=0;
        MOTOR1_PWM=0;
    }
    else
    {
        MOTOR1_DIR = MOTOR1_REVERSAL;       
        Freq=5000000/MOVEspd;             
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(PulseNum < Motor1.Limit_Pulse*2)
        { 
            if(MOTOR1_HOME) 
                break;
        }
        if(PulseNum ==Motor1.Limit_Pulse*2||PulseNum >Motor1.Limit_Pulse*2) { Motor_Status=11;}
        T5CONbits.TON=0;      
        Freq=5000000/LEAVspd;   
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;       
        while(PulseNum < Motor1.Overshoot_Pulse*2); 
        T5CONbits.TON=0;       		
    	Delay1ms(1000);
        MOTOR1_DIR = MOTOR1_FORWARD;    
        Freq=5000000/LEAVspd;          
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;
        while(MOTOR1_HOME)
        {
            if(PulseNum >= Motor1.Limit_Pulse*2) { Motor_Status=11;break;}
        }
        T5CONbits.TON=0;
        MOTOR1_PWM=0;
    }
    MOTOR1_DIR = MOTOR1_FORWARD; 
    Freq=5000000/LEAVspd;          
    TMR5=0;
    PR5=Freq;
    IFS1bits.T5IF = 0;
    T5CONbits.TON=1;
    PulseNum=0;
    while(!MOTOR1_HOME)
    {
        if(PulseNum >= pulseoffset*2 ) break;
    }
    T5CONbits.TON=0;
    MOTOR1_PWM=0;
    InitMotor1PWM();
    g_uliRunningPulse = 0;
    g_tReagentMotorParam.ulXiPosition =0;  
}
void Motor2Home(unsigned int pulseoffset,unsigned int MOVEspd,unsigned int LEAVspd )
{
    unsigned int Freq;
    Motor1_HOME_Flag=0;
    Motor2_HOME_Flag=1;
    Motor3_HOME_Flag=0;
    Motor4_HOME_Flag=0;
    Motor5_HOME_Flag=0;
    OC2CON = 0;
    g_tReagentMotorParam.ucZDirection =  MOTOR2_FORWARD;
    Delay1ms(2);
    T5CONbits.TON=0;
    TMR5=0;
	PulseNum=0;
    if(MOTOR2_HOME == 1)
    {
        MOTOR2_DIR =MOTOR2_FORWARD ;
        Freq=5000000/LEAVspd;       
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(MOTOR2_HOME)        
        {
            if(PulseNum > Motor2.Limit_Pulse*2) {Motor_Status=21;break;}
        }
        T5CONbits.TON=0;
        MOTOR2_PWM=0;
    }
    else
    {
        MOTOR2_DIR = MOTOR2_REVERSAL; 
        Freq=5000000/MOVEspd;      
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(PulseNum < Motor2.Limit_Pulse*2)
        { 
            if(MOTOR2_HOME)   
                break;
        }
        if(PulseNum ==Motor2.Limit_Pulse*2||PulseNum >Motor2.Limit_Pulse*2) { Motor_Status=21;}        
        T5CONbits.TON=0;        
        Freq=5000000/LEAVspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;        
        while(PulseNum < Motor2.Overshoot_Pulse*2); 
        T5CONbits.TON=0;        	
    	Delay1ms(1000);
        MOTOR2_DIR = MOTOR2_FORWARD;            
        Freq=5000000/LEAVspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;
        while(MOTOR2_HOME)
        {
            if(PulseNum > Motor2.Limit_Pulse*2)  {Motor_Status=21;break;}
        }
        T5CONbits.TON=0;
        MOTOR2_PWM=0;
    }
    MOTOR2_DIR = MOTOR2_FORWARD;              
    Freq=5000000/LEAVspd;
    TMR5=0;
    PR5=Freq;
    IFS1bits.T5IF = 0;
    T5CONbits.TON=1;
    PulseNum=0;
    while(!MOTOR2_HOME)
    {
        if(PulseNum >= pulseoffset*2 ) break;   
    }
    T5CONbits.TON=0;
    MOTOR2_PWM=0;
    InitMotor2PWM();
    g_uliRunningPulse = 0;
    g_tReagentMotorParam.ulZiPosition =0;
}
void Motor3Home(unsigned int pulseoffset,unsigned int MOVEspd,unsigned int LEAVspd )
{
    unsigned int Freq;
    Motor1_HOME_Flag=0;
    Motor2_HOME_Flag=0;
    Motor3_HOME_Flag=1;
    Motor4_HOME_Flag=0;
    Motor5_HOME_Flag=0;
    OC3CON = 0;
    g_tReagentMotorParam.ucZ1Direction = MOTOR3_FORWARD;
    Delay1ms(2);
    T5CONbits.TON=0;
    TMR5=0;
    PulseNum=0;
    if(MOTOR3_HOME == 1)
    {
        MOTOR3_DIR = MOTOR3_FORWARD;  
        Freq=5000000/LEAVspd;        
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(MOTOR3_HOME) 
        {
            if(PulseNum > Motor3.Limit_Pulse*2) {Motor_Status=31;break;}
        }
        T5CONbits.TON=0;
        MOTOR3_PWM=0;
    }
    else
    {
        MOTOR3_DIR = MOTOR3_REVERSAL;
        Freq=5000000/MOVEspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(PulseNum < Motor3.Limit_Pulse*2)
        { 
            if(MOTOR3_HOME)  
                break;
        }
        if(PulseNum ==Motor3.Limit_Pulse*2||PulseNum >Motor3.Limit_Pulse*2) { Motor_Status=31;}         
        T5CONbits.TON=0;      
        Freq=5000000/LEAVspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;        
        while(PulseNum < Motor3.Overshoot_Pulse*2); 
        T5CONbits.TON=0;    		
    	Delay1ms(1000);
        MOTOR3_DIR = MOTOR3_FORWARD; 
        Freq=5000000/LEAVspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;
        while(MOTOR3_HOME)
        {
            if(PulseNum > Motor3.Limit_Pulse*2)  {Motor_Status=31;break;}
        }
        T5CONbits.TON=0;
        MOTOR3_PWM=0;
    }
    MOTOR3_DIR = MOTOR3_FORWARD; 
    Freq=5000000/LEAVspd;
    TMR5=0;
    PR5=Freq;
    IFS1bits.T5IF = 0;
    T5CONbits.TON=1;
    PulseNum=0;
    while(!MOTOR3_HOME)
    {
        if(PulseNum >= pulseoffset*2 ) break;
    }
    T5CONbits.TON=0;
    MOTOR3_PWM=0;
    InitMotor3PWM();
    g_uliRunningPulse = 0;
    g_tReagentMotorParam.ulZ1iPosition =0;
}
void Motor4Home(unsigned int pulseoffset,unsigned int MOVEspd,unsigned int LEAVspd )
{
    unsigned int Freq;
    Motor1_HOME_Flag=0;
    Motor2_HOME_Flag=0;
    Motor3_HOME_Flag=0;
    Motor4_HOME_Flag=1;
    Motor5_HOME_Flag=0;
    OC4CON = 0;
    g_tReagentMotorParam.uc4Direction = MOTOR4_FORWARD;
    Delay1ms(2);
    T5CONbits.TON=0;
    TMR5=0;
    PulseNum=0;
    if(MOTOR4_HOME == 1)
    {
        MOTOR4_DIR = MOTOR4_FORWARD; 
        Freq=5000000/LEAVspd;       
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(MOTOR4_HOME)  
        {
            if(PulseNum > Motor4.Limit_Pulse*2) {Motor_Status=41;break;}
        }
        T5CONbits.TON=0;
        MOTOR4_PWM=0;
    }
    else
    {
        MOTOR4_DIR = MOTOR4_REVERSAL;
        Freq=5000000/MOVEspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        while(PulseNum < Motor4.Limit_Pulse*2)
        { 
            if(MOTOR4_HOME)  
                break;
        }
        if(PulseNum ==Motor4.Limit_Pulse*2||PulseNum >Motor4.Limit_Pulse*2) { Motor_Status=41;}         
        T5CONbits.TON=0;      
        Freq=5000000/LEAVspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;        
        while(PulseNum < Motor4.Overshoot_Pulse*2);  
        T5CONbits.TON=0;    		
    	Delay1ms(1000);
        MOTOR4_DIR = MOTOR4_FORWARD; 
        Freq=5000000/LEAVspd;
        TMR5=0;
        PR5=Freq;
        IFS1bits.T5IF = 0;
        T5CONbits.TON=1;
        PulseNum=0;
        while(MOTOR4_HOME)
        {
            if(PulseNum > Motor4.Limit_Pulse*2)  {Motor_Status=41;break;}
        }
        T5CONbits.TON=0;
        MOTOR4_PWM=0;
    }
    MOTOR4_DIR = MOTOR4_FORWARD; 
    Freq=5000000/LEAVspd;
    TMR5=0;
    PR5=Freq;
    IFS1bits.T5IF = 0;
    T5CONbits.TON=1;
    PulseNum=0;
    while(!MOTOR4_HOME)
    {
        if(PulseNum >= pulseoffset*2 ) break;
    }
    T5CONbits.TON=0;
    MOTOR4_PWM=0;
    InitMotor4PWM();
    g_uliRunningPulse = 0;
    g_tReagentMotorParam.ul4iPosition =0;
}


void Motor1MoveTo(unsigned long int pulse,unsigned int spd)
{
    unsigned long int uliMotorPulse = 0;
        if(pulse > g_tReagentMotorParam.ulXiPosition)
        {
            uliMotorPulse = pulse - g_tReagentMotorParam.ulXiPosition;
            Motor1Move(uliMotorPulse,!Motor1.Reset_Direction,spd);
        }
        else if(pulse < g_tReagentMotorParam.ulXiPosition)
        {
            uliMotorPulse = g_tReagentMotorParam.ulXiPosition - pulse;
            Motor1Move(uliMotorPulse,Motor1.Reset_Direction,spd);
        }
        else
        {

        }
}
void Motor2MoveTo_ok(unsigned long int pulse,unsigned int spd)
{
    if(Z_di_bu_position>21000)
    {
        pulse=pulse+(Z_di_bu_position-21000);
    }
    else
    {
        pulse=pulse-(21000-Z_di_bu_position);
    }
    Motor2MoveTo(pulse,spd);
}

void Motor2MoveTo(unsigned long int pulse,unsigned int spd)
{
    unsigned long int uliMotorPulse = 0;
    if(pulse > g_tReagentMotorParam.ulZiPosition)
    {
        uliMotorPulse = pulse - g_tReagentMotorParam.ulZiPosition;
        Motor2Move(uliMotorPulse,!Motor2.Reset_Direction,spd);
    }
    else if(pulse < g_tReagentMotorParam.ulZiPosition)
    {
        uliMotorPulse = g_tReagentMotorParam.ulZiPosition - pulse;
        Motor2Move(uliMotorPulse,Motor2.Reset_Direction,spd);
    }
    else
    {
        
    }
}
void Motor3MoveTo(unsigned long int pulse,unsigned int spd)
{
    unsigned long int uliMotorPulse = 0;
    if(pulse > g_tReagentMotorParam.ulZ1iPosition)
    {
        uliMotorPulse = pulse - g_tReagentMotorParam.ulZ1iPosition;
        Motor3Move(uliMotorPulse,!Motor3.Reset_Direction,spd);
    }
    else if(pulse < g_tReagentMotorParam.ulZ1iPosition)
    {
        uliMotorPulse = g_tReagentMotorParam.ulZ1iPosition - pulse;
        Motor3Move(uliMotorPulse,Motor3.Reset_Direction,spd);
    }
    else
    {
        
    }
}
void Motor4MoveTo(unsigned long int pulse,unsigned int spd)
{
    unsigned long int uliMotorPulse = 0;
    if(pulse > g_tReagentMotorParam.ul4iPosition)
    {
        uliMotorPulse = pulse - g_tReagentMotorParam.ul4iPosition;
        Motor4Move(uliMotorPulse,!Motor4.Reset_Direction,spd);
    }
    else if(pulse < g_tReagentMotorParam.ul4iPosition)
    {
        uliMotorPulse = g_tReagentMotorParam.ul4iPosition - pulse;
        Motor4Move(uliMotorPulse,Motor4.Reset_Direction,spd);
    }
    else
    {
        
    }
}



/*******************************************************************/
void Motor1Move(unsigned long int pulse,unsigned char dir,unsigned int spd)
{       
    if(g_tReagentMotorParam.ulZiPosition>Z_limitation1||g_tReagentMotorParam.ulZiPosition>Z1_limitation1)
    {
        if(X_move_protection_flag==0)
        {
            Motor_Status=13;
            return;
        }
    }
    InitMotor1PWM();
    if(pulse ==0) return;  
    MOTOR1_DIR = dir;
    if(dir==MOTOR1_FORWARD)
    {
         if(( g_tReagentMotorParam.ulXiPosition+pulse)>Motor1.Limit_Pulse)     
         {
             Motor_Status=12;
             return;
         }
    }
    if(dir==MOTOR1_REVERSAL)
    {
         if( g_tReagentMotorParam.ulXiPosition<pulse)
         {
             Motor_Status=12;
             return;
         }
    }    
    g_uiAccIndex=0;
    g_uliRunningPulse=0;
    g_ucMotorXFinishFlag = 0;
    g_uliPulseCount = pulse;
    g_tReagentMotorParam.ucXDirection=dir?1:0;
    if(spd>REAGENT_MOTOR_FREQMIN) 
    {
         if(spd>REAGENT_MOTOR_FREQMAX)
         {
             spd=REAGENT_MOTOR_FREQMAX;
         }           
        SetMotor1(pulse,spd,&(g_tReagentMotorParam.ulXiAccPulse),&(g_tReagentMotorParam.ulXiConstPulse));
        g_tReagentMotorParam.uiXSpeed_switch=0;
        g_tReagentMotorParam.uiXSpeed=spd;
    }
    else 
    {
         g_tReagentMotorParam.uiXSpeed_switch=1;
         if(spd<MOTOR_FREQLIMIT_L)
         {
             spd=MOTOR_FREQLIMIT_L;
         }
         if(spd>MOTOR_FREQLIMIT_H) 
         {
             spd=MOTOR_FREQLIMIT_H; 
         }
          g_tReagentMotorParam.uiXSpeed=spd;
          g_tReagentMotorParam.uiXprd=(unsigned short)(625000.0/ g_tReagentMotorParam.uiXSpeed);
     }
    IEC0bits.OC1IE = 1;    
    T2CONbits.TON=1;                     
    while(!g_ucMotorXFinishFlag); 
    OC1CON=0;
}

void Motor2Move(unsigned long int pulse,unsigned char dir,unsigned int spd)
{   
    InitMotor2PWM();
    if(pulse ==0) return;
    MOTOR2_DIR = dir;//r
    g_tReagentMotorParam.ucZDirection = dir?1:0;  
    if(dir==MOTOR2_FORWARD)
    {
         if(( g_tReagentMotorParam.ulZiPosition+pulse)>Motor2.Limit_Pulse)
         if( g_tReagentMotorParam.ulXiPosition<pulse)
         {
             Motor_Status=22;
             return;
         }
    }
    if(dir==MOTOR2_REVERSAL)
    {
         if( g_tReagentMotorParam.ulZiPosition<pulse)
         if( g_tReagentMotorParam.ulXiPosition<pulse)
         {
             Motor_Status=22;
             return;
         }
    }

    g_uiAccIndex=0;
    g_uliRunningPulse=0;
    g_ucMotorZFinishFlag = 0;
    g_uliPulseCount = pulse;
    if(spd>REAGENT_MOTOR_FREQMIN2) 
    {      
         if(spd>Motor2.Limit_speed)
         {
             spd=Motor2.Limit_speed;
         }        
        SetMotor2(pulse,spd,&(g_tReagentMotorParam.ulZiAccPulse),&(g_tReagentMotorParam.ulZiConstPulse));
        g_tReagentMotorParam.uiZSpeed_switch=0;
        g_tReagentMotorParam.uiZSpeed=spd;
    }
    else 
    {
         g_tReagentMotorParam.uiZSpeed_switch=1;
         if(spd<MOTOR_FREQLIMIT_L)
         {
             spd=MOTOR_FREQLIMIT_L;
         }
         if(spd>MOTOR_FREQLIMIT_H) 
         {
             spd=MOTOR_FREQLIMIT_H; 
         }
          g_tReagentMotorParam.uiZSpeed=spd;
          g_tReagentMotorParam.uiZprd=(unsigned short)(625000.0/ g_tReagentMotorParam.uiZSpeed);
     }  
    IEC0bits.OC2IE = 1;
    T2CONbits.TON=1;            
    while(!g_ucMotorZFinishFlag); 
    OC2CON=0;
}

void Motor3Move(unsigned long int pulse,unsigned char dir,unsigned int spd)//spd --HZ
{   
    InitMotor3PWM();
    if(pulse ==0) return;
    MOTOR3_DIR = dir;
   if(dir==MOTOR3_FORWARD)
    {
         if(( g_tReagentMotorParam.ulZ1iPosition+pulse)>Motor3.Limit_Pulse)
         {
             Motor_Status=32;
             return;
         }
    }
    if(dir==MOTOR3_REVERSAL)
    {
         if( g_tReagentMotorParam.ulZ1iPosition<pulse)
         {
             Motor_Status=32;
             return;
         }
    }
    g_tReagentMotorParam.ucZ1Direction = dir?1:0;
    g_uiAccIndex=0;
    g_uliRunningPulse=0;
    g_ucMotorZ1FinishFlag = 0;
    g_uliPulseCount = pulse;
 if(spd>REAGENT_MOTOR_FREQMIN3) 
    {
         if(spd>REAGENT_MOTOR_FREQMAX3)
         {
             spd=REAGENT_MOTOR_FREQMAX3;
         }               
        SetMotor3(pulse,spd,&(g_tReagentMotorParam.ulZ1iAccPulse),&(g_tReagentMotorParam.ulZ1iConstPulse));
        g_tReagentMotorParam.uiZ1Speed_switch=0;
        g_tReagentMotorParam.uiZ1Speed=spd;
    }
    else 
    {
         g_tReagentMotorParam.uiZ1Speed_switch=1;
         if(spd<MOTOR_FREQLIMIT_L)
         {
             spd=MOTOR_FREQLIMIT_L;
         }
         if(spd>MOTOR_FREQLIMIT_H) 
         {
             spd=MOTOR_FREQLIMIT_H; 
         }
          g_tReagentMotorParam.uiZ1Speed=spd;
          g_tReagentMotorParam.uiZ1prd=(unsigned short)(625000.0/ g_tReagentMotorParam.uiZ1Speed);
     }  
    IEC1bits.OC3IE = 1;    
    T2CONbits.TON=1;              //
    while(!g_ucMotorZ1FinishFlag); 
     OC3CON=0;
}

void Motor4Move(unsigned long int pulse,unsigned char dir,unsigned int spd)//spd --HZ
{   
    InitMotor4PWM();
    if(pulse ==0) return;
    MOTOR4_DIR = dir;
   if(dir==MOTOR4_FORWARD)
    {
         if(( g_tReagentMotorParam.ul4iPosition+pulse)>Motor4.Limit_Pulse)
         {
             Motor_Status=42;
             return;
         }
    }
    if(dir==MOTOR4_REVERSAL)
    {
         if( g_tReagentMotorParam.ul4iPosition<pulse)
         {
             Motor_Status=42;
             return;
         }
    }
    g_tReagentMotorParam.uc4Direction = dir?1:0;
    g_uiAccIndex=0;
    g_uliRunningPulse=0;
    g_ucMotor4FinishFlag = 0;
    g_uliPulseCount = pulse;
 if(spd>REAGENT_MOTOR_FREQMIN4) 
    {
         if(spd>REAGENT_MOTOR_FREQMAX4)
         {
             spd=REAGENT_MOTOR_FREQMAX4;
         }               
        SetMotor4(pulse,spd,&(g_tReagentMotorParam.ul4iAccPulse),&(g_tReagentMotorParam.ul4iConstPulse));
        g_tReagentMotorParam.ui4Speed_switch=0;
        g_tReagentMotorParam.ui4Speed=spd;
    }
    else 
    {
         g_tReagentMotorParam.ui4Speed_switch=1;
         if(spd<MOTOR_FREQLIMIT_L)
         {
             spd=MOTOR_FREQLIMIT_L;
         }
         if(spd>MOTOR_FREQLIMIT_H) 
         {
             spd=MOTOR_FREQLIMIT_H; 
         }
          g_tReagentMotorParam.ui4Speed=spd;
          g_tReagentMotorParam.ui4prd=(unsigned short)(625000.0/ g_tReagentMotorParam.ui4Speed);
     }  
    IEC1bits.OC4IE = 1;    
    T2CONbits.TON=1;              //
    while(!g_ucMotor4FinishFlag); 
     OC4CON=0;
}

void Move_to_by_volume(unsigned  int volume,unsigned int spd)//spd --HZ
{
    unsigned int pulse;
    if(volume<40)
    {
        pulse=(unsigned int)(Z_di_bu_position-(volume*12.5+0));
    }
    else if(volume<400)
    {
         pulse=(unsigned int)(Z_di_bu_position-(volume*10.05139057+103.5066505));
    }
    else if(volume<1100)
    {
         pulse=(unsigned int)(Z_di_bu_position-(volume*6.833333333+1366.666667));
    }
    else
    {
        return;
    }
    Motor2MoveTo( pulse, spd);
}