main.c

飞思卡尔智能车程序

#include <hidef.h>      /* common defines and macros */
#include "derivative.h"      /* derivative-specific definitions */
#include"main.h"
#include"init.h"

void AD_collect(void)
{
  while(ATDSTAT0_SCF==0); //等待转换结束
  nInputVoltage[0] += ATDDR0;
  nInputVoltage[1] += ATDDR1;
  nInputVoltage[2] += ATDDR2;
  nInputVoltage[3] += ATDDR3;
  nInputVoltage[4] += ATDDR4;
  ATDSTAT0_SCF=1;
}


void GetInputVoltageAverage(void){
  int i;
  for(i=0;i<20;i++){
  AD_collect();
  }
	for(i=0;i<5;i++){
			//	g_lnInputVoltageSigma[i] += nInputVoltage[i];
				g_nInputVoltage[i] = (int)(nInputVoltage[i]/20 );
				nInputVoltage[i]=0;
			}
	
} 


void GetMotorPulse(void) {
	unsigned int nLeftPulse, nRightPulse;
	
	
	nLeftPulse=PACNT;
	PACNT = 0;
	nRightPulse=PORTD;
	PORTC=0xff;
	//_asm(nop);
	//_asm(nop);
	//_asm(nop);
	PORTC=0x00;
	
	g_nLeftMotorPulse = (int)nLeftPulse;
	g_nRightMotorPulse = (int)nRightPulse;
if(g_fLeftMotorOut<0&&g_fRightMotorOut<0){
		g_nLeftMotorPulse = -g_nLeftMotorPulse;
		g_nRightMotorPulse = -g_nRightMotorPulse;
		}

	g_nLeftMotorPulseSigma += g_nLeftMotorPulse;
  g_nRightMotorPulseSigma += g_nRightMotorPulse;
}

void stopcar(void) {// 停车程序
SetMotorVoltage (0,0);
}


void main(void) {
  /* put your own code here */
  int i;
  system_init();
 for(i=0;i<2000;i++){
   AD_collect();
}
  LEFT_OFFSETGET=(int)(nInputVoltage[0]/2000);
  RIGHT_OFFSETGET=(int)(nInputVoltage[1]/2000);
  GYROSCOPE_OFFSETGET=nInputVoltage[3]/2000;
  for(i=0;i<5;i++){
       nInputVoltage[i]=0;
  } 
  
	EnableInterrupts;


  for(;;) {
   
    _FEED_COP(); /* feeds the dog */
  } /* loop forever */
  /* please make sure that you never leave main */
}



/*---------------------直立控制-------------------*/
void AngleCalculate(void) {
  
	float fDeltaValue;
	
	//--------------------------------------------------------------------------
	// Define Angle 
	g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO;

	g_fGyroscopeAngleSpeed= (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;
	
	g_fCarAngle = g_fGyroscopeAngleIntegral;
	fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
	//	fDeltaValue =  g_fCarAngle / GRAVITY_ADJUST_TIME_CONSTANT;
//	g_fGyroscopeAngleIntegral += g_fGyroscopeAngleSpeed  / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;	
	g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed +fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
 
} 
/*void AngleCalculate(void){
    g_fCarAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO;
    g_fGyroscopeAngleSpeed= (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;
    
}*/                         

void AngleControl(void) {
  float fValue;
	fValue = (CAR_ANGLE_SET - g_fCarAngle) * ANGLE_CONTROL_P +
	         (CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D;

	g_fAngleControlOut = fValue;
	
}



/*------------------------速度控制--------------------*/
void SpeedControl(void) {
	float fP, fDelta;
	float fI;
	g_fCarSpeed = (g_nLeftMotorPulseSigma + g_nRightMotorPulseSigma) / 2;
	g_nLeftMotorPulseSigma = g_nRightMotorPulseSigma = 0;
	g_fCarSpeed *= CAR_SPEED_CONSTANT;

	fDelta = CAR_SPEED_SET;
	fDelta -= g_fCarSpeed;
	fP = fDelta * SPEED_CONTROL_P;
	fI = fDelta * SPEED_CONTROL_I;
		//g_fSpeedControlIntegral= fI;


		  
	  g_fSpeedControlIntegral += fI;

	
		g_fSpeedControlOutOld = g_fSpeedControlOutNew;

	g_fSpeedControlOutNew = fP + g_fSpeedControlIntegral;
}

void SpeedControlOutput(void) {
	float fValue;
	fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld;
	g_fSpeedControlOut = fValue * (g_nSpeedControlPeriod + 1) / SPEED_CONTROL_PERIOD + g_fSpeedControlOutOld;
//	if(g_fSpeedControlOut>SpeedControlOutMAX) g_fSpeedControlOut=SpeedControlOutMAX;
//	if(g_fSpeedControlOut<SpeedControlOutMIN) g_fSpeedControlOut=SpeedControlOutMIN; 
}


/*-------------------方向控制-----------------------------*/
void DirectionVoltageSigma(void) {
	int nLeft, nRight;
	if(VOLTAGE_LEFT > DIR_LEFT_OFFSET) 		nLeft = VOLTAGE_LEFT - DIR_LEFT_OFFSET;
	else nLeft = 0;
	if(VOLTAGE_RIGHT > DIR_RIGHT_OFFSET) 	nRight = VOLTAGE_RIGHT - DIR_RIGHT_OFFSET;
	else nRight = 0;

	g_fLeftVoltageSigma += nLeft;
	g_fRightVoltageSigma += nRight;
}

void DirectionControl(void) {
	float fLeftRightAdd, fLeftRightSub, fValue;
	float fDValue;
	int nLeft, nRight;

	nLeft = (int)(g_fLeftVoltageSigma /= DIRECTION_CONTROL_COUNT);
	nRight = (int)(g_fRightVoltageSigma /= DIRECTION_CONTROL_COUNT);
	g_fLeftVoltageSigma = 0;
	g_fRightVoltageSigma = 0;	

	fLeftRightAdd = nLeft + nRight;
	fLeftRightSub = nLeft - nRight;
	
	g_fDirectionControlOutOld = g_fDirectionControlOutNew;
	
	if(fLeftRightAdd < LEFT_RIGHT_MINIMUM) {
		g_fDirectionControlOutNew = 0;
	} else {

		fValue = fLeftRightSub * DIR_CONTROL_P / fLeftRightAdd;
    fDValue = DIR_CONTROL_D_VALUE-DIRECTION_OFFSET; 

    fDValue *= DIR_CONTROL_D;
    
    fValue += fDValue;
    
			g_fDirectionControlOutNew = fValue;
	}
}

void DirectionControlOutput(void) {
	float fValue;
	fValue = g_fDirectionControlOutNew-g_fDirectionControlOutOld;
	g_fDirectionControlOut = fValue * (g_nDirectionControlPeriod + 1)/DIRECTION_CONTROL_PERIOD + g_fDirectionControlOutOld;
	
}




/*-------------------电机输出设置-------------------*/
void MotorOutput(void) {
	float fLeft, fRight;

	fLeft = g_fAngleControlOut-g_fSpeedControlOut+g_fDirectionControlOut;//- g_fDirectionControlOut;
	fRight = g_fAngleControlOut-g_fSpeedControlOut- g_fDirectionControlOut;//+ g_fDirectionControlOut;
	
	if(fLeft > MOTOR_OUT_MAX)		fLeft = MOTOR_OUT_MAX;
	if(fLeft < MOTOR_OUT_MIN)		fLeft = MOTOR_OUT_MIN;
	if(fRight > MOTOR_OUT_MAX)		fRight = MOTOR_OUT_MAX;
	if(fRight < MOTOR_OUT_MIN)		fRight = MOTOR_OUT_MIN;
		
	g_fLeftMotorOut = fLeft;
	g_fRightMotorOut = fRight;
	MotorSpeedOut();
}

void MotorSpeedOut(void) {
	float fLeftVal, fRightVal;
	
	fLeftVal = g_fLeftMotorOut;
	fRightVal = g_fRightMotorOut;
	if(fLeftVal > 0) 			fLeftVal += MOTOR_OUT_DEAD_VAL;

	else if(fLeftVal < 0) 		fLeftVal -= MOTOR_OUT_DEAD_VAL;
	
	if(fRightVal > 0)			fRightVal += MOTOR_OUT_DEAD_VAL;
	else if(fRightVal < 0)		fRightVal -= MOTOR_OUT_DEAD_VAL;
		
	SetMotorVoltage(fLeftVal, fRightVal);
}

void SetMotorVoltage(float fLeftVoltage, float fRightVoltage) {
                                                // Voltage : > 0 : Move forward;
   int nOut;                                             //           < 0 : Move backward
	short nPeriod0=500,nPeriod1=500;
	
	//nPeriod = (short)getReg(PWM_PWMCM);
	
	//--------------------------------------------------------------------------
	if(fLeftVoltage > 2) 			fLeftVoltage = 2;
	else if(fLeftVoltage < -2) 	fLeftVoltage = -2;
	
	if(fRightVoltage > 2) 		fRightVoltage = 2;
	else if(fRightVoltage < -2)	fRightVoltage = -2;
                                              
	//--------------------------------------------------------------------------	                                            	                                              
	if(fLeftVoltage>0) {
		PWMDTY01=0;            
		nOut = (int)(fLeftVoltage * nPeriod0);
		PWMDTY23=nOut;         
	} else {
		PWMDTY23=0;            
		fLeftVoltage = -fLeftVoltage;
		nOut = (int)(fLeftVoltage * nPeriod0);

		PWMDTY01=nOut;         
	}                                     

	//--------------------------------------------------------------------------
	if(fRightVoltage>0) {
		PWMDTY67=0;            
		nOut = (int)(fRightVoltage * nPeriod1);
			//	nOut+=50;
		PWMDTY45=nOut;         
	} else {
		PWMDTY45=0;            
		fRightVoltage = -fRightVoltage;
		nOut = (int)(fRightVoltage * nPeriod1);
	//	nOut+=50;
		PWMDTY67=nOut;         
	}

	//MOTOR_SETLOAD;                              // Reload the PWM value
}



                                                                               
#pragma CODE_SEG __NEAR_SEG NON_BANKED  
interrupt  VectorNumber_Vapi void API_ISR(void)
 {
    CPMUAPICTL_APIF =1; //This flag can only be cleared by writing a 1

    g_nSpeedControlPeriod ++;
    
    //AD_collect();
     
    SpeedControlOutput();
    g_n1MSEventCount ++;
    
    g_nDirectionControlPeriod ++;
  	DirectionControlOutput();
    
 
 if(g_n1MSEventCount >= CONTROL_PERIOD) {    // Motor speed adjust
   g_n1MSEventCount = 0;
      GetMotorPulse();

    
    } else if(g_n1MSEventCount == 1) {
      GetInputVoltageAverage();
      //AD_collect();
      //SampleInputVoltage();
      
  }  if(g_n1MSEventCount == 2) {
      
      AngleCalculate();
      if(g_fCarAngle>20||g_fCarAngle<-20)  {
      stopcar();
      
  } else{
      AngleControl();
       MotorOutput();
 }

      
  }  if(g_n1MSEventCount == 3) {
        g_nSpeedControlCount ++; 
        if(g_nSpeedControlCount >= SPEED_CONTROL_COUNT) {
    if(g_fCarSpeedset<=40) {
      g_fCarSpeedset+=1;
     }
     if(g_fCarSpeedset>=40) g_fCarSpeedset=40;
     SpeedControl();
   g_nSpeedControlCount = 0;
   g_nSpeedControlPeriod= 0;
        }
 
  
  }  if(g_n1MSEventCount >= 4) {      // Motor speed adjust
  		g_nDirectionControlCount ++;
		  DirectionVoltageSigma();
  		if(g_nDirectionControlCount >= DIRECTION_CONTROL_COUNT) {
  	  		DirectionControl();
  	  		g_nDirectionControlCount = 0;
  	  		g_nDirectionControlPeriod = 0;
  		}
  } 
 }
 #pragma CODE_SEG DEFAULT