ALPS-004/WZYXPCR_NO1.20250726.X/WZYXPCR_NO1.20250726.X/file/Motor.c

#define  __MOTOR1_DEF
#include "ABIS_User.h"

#define MAX_SETP_OF_X   500000
#define MAX_SETP_OF_X_LEAVEOPT   100000
/*******************************************************************************
 * 函数名称：CalculateSModeLine
 *******************************************************************************/
void CalculateSModeLineX( 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>MOTORX_ACCLTH) len=MOTORX_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);                         
    }
}
/*******************************************************************************
 * 函数名称：InitMotorPWM
 *******************************************************************************/
void InitMotorXPWM(void){
    _RP100R = 0x10;               
    OC1CON2 = 0;                 
    OC1CON1bits.OCTSEL = 0;   

    OC1R = 15000;                

    OC1RS = 15000;
    T2CON = 0;
    T2CONbits.TCKPS = 2;         
    PR2 = 30000;                  
    OC1CON2bits.SYNCSEL = 0x0C;  
    OC1CON1bits.OCM = 5;         
    T2CONbits.TON = 0;         
    IEC0bits.T2IE = 1;            
    IFS0bits.T2IF = 0;
    IPC1bits.T2IP = 7;
}
/*******************************************************************************
 * 函数名称：SetMotor
 *******************************************************************************/
void SetMotorX(unsigned long int Pulses,unsigned int spd,unsigned long int *AccPLS,unsigned long int *constVPLS)
{
    unsigned int i;
    unsigned int Prd;
    Prd = 625000 / spd; 
    for (i = 0; i < MOTORX_ACC; i++) {
        if ((Prd < g_uiMotorXPeriod[i])&&(Prd >= g_uiMotorXPeriod[i + 1])) {
            *AccPLS = i + 1;
            break;
        } else if (Prd == g_uiMotorXPeriod[i]) {
            *AccPLS = i;
            break;
        }
    }
    if (Pulses >= *AccPLS + *AccPLS) {
        *constVPLS = Pulses - *AccPLS;      
    } else if (Pulses < *AccPLS + *AccPLS) { 
        *AccPLS = Pulses / 2;
        *constVPLS = *AccPLS;
	}	
}
/*******************************************************************************
 * 函数名称：MotorHome
 *******************************************************************************/
unsigned int MotorXHome(unsigned int pulseoffset,unsigned int MOVEspd,unsigned int LEAVspd )
{
	unsigned int Freq;
	OC1CON1 = 0;  
    MotorXHomebuffer = 1;
    MotorYHomebuffer = 0;
    MotorZHomebuffer = 0;
	Delay_ms(2);
	
    TMR8=0;
	PulseNum=0;
    if(HOMEX == 0){
        MOTORX_DIR = FORWARD;
        Freq=5000000/LEAVspd;
        TMR8=0;
        PR8=Freq;
        IFS3bits.T8IF = 0;
        T8CONbits.TON=1;
        
        while(!HOMEX){
            if(PulseNum > MAX_SETP_OF_X_LEAVEOPT) 
        		T8CONbits.TON=0;
				return 0;
        }
        T8CONbits.TON=0;
        MOTORX_PWM=0;
    }else{
        MOTORX_DIR = REVERSAL;
        Freq=5000000/MOVEspd;
        TMR8=0;
        PR8=Freq; 
        IFS3bits.T8IF = 0;
        T8CONbits.TON=1;
        while(1){ 
            if(!HOMEX) 
				break;
			if(PulseNum >= MAX_SETP_OF_X){				
        		T8CONbits.TON=0;
				return 0;
			}
        }
        T8CONbits.TON=0;
        
        Freq=5000000/LEAVspd;
        TMR8=0;
        PR8=Freq;
        IFS3bits.T8IF = 0;
        T8CONbits.TON=1;
        PulseNum=0;
        
        while(PulseNum <= g_tEE.uiXPulseHome * 2);
        T8CONbits.TON=0;
        
        MOTORX_DIR = FORWARD;

        Freq=5000000/LEAVspd;
        TMR8=0;
        PR8=Freq;
        IFS3bits.T8IF = 0;
        T8CONbits.TON=1;
        PulseNum=0;
        while(HOMEX==0){
            if(PulseNum >= MAX_SETP_OF_X_LEAVEOPT) {
        		T8CONbits.TON=0;
				return 0;
			}
        }
        T8CONbits.TON=0;
        MOTORX_PWM=0;
    }
    InitMotorXPWM();        //电机OC初始化
    g_uliRunningPulse = 0;
    g_uiScanPulseX = 0;
	g_tMotorParam.ulXiPosition =0;
	Delay_ms(10);
	return 1;
    
    
   
    
    
    
    
}
/*******************************************************************************
 * 函数名称：MotorXMoveTo 暂时没有调用
 *******************************************************************************/
void MotorXMoveTo(unsigned long int pulse,unsigned int spd)
{
    unsigned long int uliMotorPulse = 0;
    if(pulse > g_tMotorParam.ulXiPosition)
    {
        uliMotorPulse = pulse - g_tMotorParam.ulXiPosition;
        MotorXMove(uliMotorPulse,1,spd);
    }
    else if(pulse < g_tMotorParam.ulXiPosition)
    {
        uliMotorPulse = g_tMotorParam.ulXiPosition - pulse;
        MotorXMove(uliMotorPulse,0,spd);
    }
}
/*******************************************************************************
 * 函数名称：MotorXMove
 *******************************************************************************/
void MotorXMove(unsigned long int pulse,unsigned char dir,unsigned int spd)//spd --HZ
{       
    if(pulse ==0) return;
    g_tMotorParam.ucXDirection = dir;
    MOTORX_DIR = dir;
	if(spd < g_tEE.uiXSpeedMin)spd = g_tEE.uiXSpeedMin;
    if(spd > g_tEE.uiXSpeedMax)spd = g_tEE.uiXSpeedMax;
	g_uiAccIndex=0;
	g_uliRunningPulse=0;
    g_ucMotorXFinishFlag = 0;
    g_uliPulseCount = pulse;
	SetMotorX(pulse,spd,&(g_tMotorParam.ulXiAccPulse),&(g_tMotorParam.ulXiConstPulse));
	T2CONbits.TON=1;//on the T1
	while(!g_ucMotorXFinishFlag); 
 
}
/*******************************************************************************
 * 函数名称：MotorXScan
 *******************************************************************************/
void MotorXScan(unsigned long int pulse,unsigned char dir,unsigned int spd)//spd --HZ
{
    if(pulse ==0) return;
	MOTORX_DIR = dir;//close to the home sensor
    MOTORX_DIR_scan = dir;   
    g_tMotorParam.ucXDirection = dir; 
    
    
    if(Alldata==1){ 
        g_ucFAMLED = 1;
        g_ucVICLED = 1;
        g_ucROXLED = 1;
        g_ucCY5LED = 1;
        g_ucCY6LED = 1;
        g_uc390LED = 1;       

        ADC7606Enable = 1;
        FAMData_num  = 0;
        VICData_num  = 0;
        ROXData_num  = 0;
        CY5Data_num  = 0;
        CY6Data_num  = 0;
        _390Data_num = 0;
        TEMP_num = 0;

    }else{                                                                     
        if(FAMLED == 1)      MPPCnum = 0;
        else if(VICLED == 1) MPPCnum = 1;     
        else if(ROXLED == 1) MPPCnum = 2;
        else if(CY5LED == 1) MPPCnum = 3;
        else if(CY6LED == 1) MPPCnum = 4;   
        else if(_390LED == 1)MPPCnum = 5;                         
        Motstep     = 0;
        FAMData_num = 3;
    } 
	if(spd<g_tEE.uiXSpeedMin) spd=g_tEE.uiXSpeedMin;
	g_uiAccIndex         = 0;
	g_uliRunningPulse    = 0;                                                
    g_ucMotorXFinishFlag = 0;
    g_uliPulseCount = pulse;
	SetMotorX(pulse,spd,&(g_tMotorParam.ulXiAccPulse),&(g_tMotorParam.ulXiConstPulse));
	T2CONbits.TON=1;//on the T1
	while(!g_ucMotorXFinishFlag); 

}