main.c

9s12dg128b freesicale智能车程序

#include <hidef.h>      /* common defines and macros */
#include <mc9s12dg128.h>     /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"


/////////*******函数声明******//////////

void CLK_Init(void) ;
void PWM_Init(void);
void ATD_Init(void) ;
void ECT_Init(void)  ;
void PORT_Init(void) ;
void delay(int time) ;
void SpeedPID(void) ;

void DirectionPID(void);
void ADT_Sample(void);
//void PIDInit(PID *pp);
//void PIDjs(PID *pp,int error);
//signed Direction_get(void);

////////////*****变量声明*****/////////////
#define PWM0MAX 200
#define BIJIAO  100
int ENLED;
int ADT_FINISHED=0;
int mms1=0;
int SpeedBack;
int SpeedGiven;
int LastPulseCounter=0;
int SpeedCache[4]=0;
int SpeedError;
int DIRe[8]=0;
int s=0;
float out;
float Kp=100;
float Ki=10;
int en[3]={0,0,0};
float kt=0.4,km=0.4;
int u;
static const byte Channels[] = {       
128,129,130,131,132,133,134,135};


//**********初始化部分****************//

void CLK_Init(void) {
     SYNR=2;
    REFDV=1;          //BUSCLK=12MHZ   BUS_CYCLE=0.083us
}

void PWM_Init(void) {
    PWMCTL = 0x70; 		//0110 0000	45,23级联  
    PWMCAE = 0x00;    //左对齐           
    PWMPOL = 0xFF;   	//脉冲先高后低			   
    PWMCLK = 0x30; 		//0011 1111，45SA,23SB,0SA,1SA		 
    PWMPRCLK = 0x33; 	//FEN PIN=8	B,A=BUSCLK/8	   
    PWMSCLB = 75;     //SB=B/150   SB=10KHZ   
    PWMSCLA = 75; 		//SA=A/150	 SA=10KHZ
     
    PWMPER0 = 200;  	//1/10KHZ*200		20MS    
    PWMDTY0 = 0;
    PWMPER1 =200;     //1/10KHZ*200		20MS
    PWMDTY1=200;
    PWMPER23 = 200; 	//1/10KHZ*200		20MS  舵机通道
    PWMDTY23 = 0; 					 
    PWMPER45 = 5000; 	//1/10KHZ*2000		200MS		
    PWMDTY45 = 700; 		 
    PWME = 0x03;      //电机接0，1通道	
}
    	
void ATD_Init(void) {
    ATD0CTL2=0xC0;   //1100 0000
    delay(100);					 
    ATD0CTL3=0x80;     //队列长度为1
    ATD0CTL4 = 0xE6;   //1110 0110 8位精度，16个A/D采样周期,128分频 T=10.6us
    ATD0DIEN = 0x00;   
}

void ECT_Init(void) {
    TIOS = 0x00; 
    PBCTL=0x62;//PBCTL_PBEN_MASK|PBCTL_PBOVI_MASK;
    PACN10=0x00;
//  TCTL4 = TCTL4_EDG0A_MASK|TCTL4_EDG1B_MASK|TCTL4_EDG2A_MASK;  
//  TSCR2 = 0x00;   
//  TIE_C0I = 1;                              
//  TIE_C1I = 1;
//  TIE_C2I = 0; 
    TSCR1 = 0x80;
    TSCR2=TSCR2_TOI_MASK; 			  
  
    MCCTL_MODMC = 0;
    MCCTL = 0x87;   //16分频
    MCCNT = 750;    //750*16/12MHZ=1ms

}

void PORT_Init(void) {
    DDRA=0x00;					 			   
    DDRB=0x00;
//  DDRJ=
//  DDRH=
//  DDRK=					  
//  DDRT=

}

void delay(int time) {
    int i,j;
    for (i=0;i<=125;i++)     //10.37us
    for(j=0;j<=time;j++);

}
////////////******模数减中断子程序*******//////////////

#pragma CODE_SEG NON_BANKED
  interrupt 26 void MDC_ISR(void){
    MCFLG_MCZF = 1;
    if(mms1++==10)mms1=0;
  } 

#pragma CODE_SEG DEFAULT

/////***********速度PID控制*************/////

void SpeedPID(void) {
    int Ctl_P = 0;
		static int Ctl_I;
		int PWM1input;
		
	  SpeedBack = PACN32 - LastPulseCounter;	
	           
	  LastPulseCounter = SpeedBack;
    
    SpeedCache[3]=SpeedCache[2];
    SpeedCache[2]=SpeedCache[1];
    SpeedCache[1]=SpeedCache[0];
    SpeedCache[0]=SpeedBack;
    
    SpeedBack = (SpeedBack +SpeedCache[1]) / 2;					   
    SpeedError = SpeedGiven - SpeedBack;

		Ctl_P = Kp * SpeedError;		                    
  	Ctl_I = Ki * SpeedError + Ctl_I;    
		if(Ctl_I < -1000) 														  
		    Ctl_I = -1000;
    else if(Ctl_I > 1000)
        Ctl_I = 1000;    
	  PWM1input	= Ctl_P + Ctl_I; 
	  
    if(PWM1input > PWM0MAX){                        
        PWM1input = PWM0MAX;
    }				
    else if(PWM1input<0)
   		  PWM1input =0;
    
    PWMDTY0 = (byte)PWM1input;
    PWMDTY1=0;

}

//////////********舵机控制PID程序*****//////

void ADT_Sample(void) {
    int SumChan;
    int Direction;
    int t;
   for ( SumChan=0; SumChan<8; SumChan++) {
    ATD0CTL5 = Channels[SumChan];      
    while (!ATD0STAT0_SCF);            
    DIRe[SumChan] = ATD0DR0L;
    if(DIRe[SumChan]>BIJIAO)DIRe[SumChan]=1;
    else DIRe[SumChan]=0;
    ATD0STAT0_SCF=1;
    }
   
//    ENLED=0;
   Direction=DIRe[0]+2*DIRe[1]+4*DIRe[2]+8*DIRe[3]+16*DIRe[4]+32*DIRe[5]+64*DIRe[6]+DIRe[7];
  t=Direction;
	if(t==0x18)
	{
		s=0;
	} 
	else if(t==0x08)
	{
		s=1;
	}
	else if(t==0x0c)
	{
		s=2;
	}
	else if(t==0x04)
	{
		s=3;
	}
	else if(t==0x06)
	{
		s=4;
	}
	else if(t==0x02)
	{
		s=5;
	}
	else if(t==0x03)
	{
		s=6;
	}
	else if(t==0x01)
	{
		s=7;
	}
	else if(t==0x10)
	{
		s=-1;
	}
	else if(t==0x30)
	{
		s=-2;
	}
	else if(t==0x20)
	{
		s=-3;
	}
	else if(t==0x60)
	{
		s=-4;
	}
	else if(t==0x40)
	{
		s=-5;
	}
	else if(t==0xc0)
	{
		s=-6;
	}
	else if(t==0x80)
	{
		s=-7;
	}

}

void DirectionPID(void)
{
	en[0]=en[1];
  en[1]=en[2];
  en[2]=s;


u=1*(en[2]-en[1])+2*en[2]+2*((en[2]-en[1])-(en[1]-en[0]));



PWMDTY23=(int)(kt*u+21);//舵机控制

if(PWMDTY23>26)PWMDTY23=26;
if(PWMDTY23<18)PWMDTY23=18;

}
/*void PIDInit(PID *pp)
{
	memset(pp,0,sizeof(PID));
}

void DirectionPID() {
    
    out=PIDjs(&sPID,s);
  	PWMDTY23=(int)(0.4*out+100);
   
}  */
///////////*******主程序*******////////////

void main(void) {
  /* put your own code here */
 /* typedef struct PID
{
	char p;
	char i;
	char d;
	int lasterror;
	int preverror;
	int sumerror;
}PID; */
  DisableInterrupts;
  CLK_Init();
  PWM_Init();
  ATD_Init();
  ECT_Init();
  PORT_Init();

  
//  PID sPID;
/*	int out;
	PIDInit(&sPID);
	sPID.p=2;
	sPID.i=2;
	sPID.d=2; 
	
*/	
  EnableInterrupts;
/* 初始速度跟舵机角度 
  PWMDTY0=
  PWMDTY1=
  PWMDTY23=
*/
  for(;;) {
    if(mms1==10) {
    mms1=0;
    DirectionPID();
    SpeedPID();
   // ENLED=;
    ENLED=1;//开采样开关
    ADT_FINISHED=0;
    delay(100);
    
    } 
    else if(ADT_FINISHED==0) {
    ADT_FINISHED=1;
    ADT_Sample();
    }
   
    
  } /* wait forever */
  /* please make sure that you never leave this function */
}