*********************************************************************************** 

* 系統(tǒng)初始化函數(shù) 

* 

* 程序描述: 初始化了系統(tǒng)時鐘，F(xiàn)LASH 和 EEPRO的工作頻率，PWM輸出口，定時器，以及PID算法中的有關(guān)參數(shù) 

* 

* 注意： 這個函數(shù)調(diào)用了 Init_PWMout()nit_INT_Timer()nit_PID()　　 

* 

* 說明： 無 

**********************************************************************************/ 

void Init(void) 

{ 

REFDV=0x01; /* initiate PLL clock */ 

SYNR =0x02; /* system clock 24M */ 

while (!(CRGFLG & 0x08)){} /* wait untill steady */ 

CLKSEL=0x80; /* 選定所相環(huán)時鐘 */ 

FCLKDIV=0x49; /* 使FLASH 和 EEPROM */ 

/* 的擦除操作工作頻率在200HZ左右 */ 

ECLKDIV=0x49; 

Init_PWMout(); /* 01:50Hz 45:1kHz */ 

Init_INT_Timer(); /* initiate ETC(Enhanced Capture Clock) */ 

Init_PID(); /* initiate PID caculating process */ 

DDRK |= 0x80; /* Start Car -- stop car */ 

PORTK &= 0x7F; 

} 

/* 

*********************************************************************************** 

* SmartCar.c 

* 

* (c) Copyright 2006,Zhao Cheng 

* All Rights Reserved 

* 

* By : Zhao Cheng 

* Data : 2006_5_6 

* Note : Don't change this file if possible. 

**********************************************************************************/ 

#include <hidef.h> 

#include <mc9s12dg128.h> 

extern SYSCLOCK; /* 引用全局變量,系統(tǒng)時鐘 */ 

void CarMain(void); 

/* 

*********************************************************************************** 

* PWM初始化函數(shù) 

* 

**********************************************************************************/ 

void Init_PWMout(void) 

{ 

PWME = 0x22; /*01:50Hz 45:1kHz */ 

PWMPOL = 0x22; 

PWMCTL = 0x50; 

PWMCLK = 0x02; 

PWMSCLA = 4; 

} 

/* 

*********************************************************************************** 

* PWM輸出函數(shù) 

* 程序描述：輸入?yún)?shù)為方向，速度 

* 方向：－45～45 

* 速度：0～24000 

**********************************************************************************/ 

void PWMout(int Direction, int Velocity) 

{ 

Direction = Direction/3 + 4500; 

if(Direction<3000) Direction=3000; 

if(Direction>6000) Direction=6000; 

PWMPER01 = 60000; /* Center 1500ms*3 */ 

PWMDTY01 = Direction+93; /* 設置舵機角度 */ 

if(Velocity>24000) Velocity=24000; 

PWMPER45 = 24000; /* 1kHz ( <10kHz ) */ 

PWMDTY45 = Velocity; /* 設置電機速度 */ 

} 

/* initiate Real Time Interrupt 1.0 */ 

void Init_INT_RTI(void) 

{ 

RTICTL = 0x74; 

CRGINT |=0x80; 

} 

/* Real Time Interrupt 1.0 */ 

interrupt void INT_RTI(void) 

{ 

CRGFLG |= 0x80; /* clear the interrrupt flag */ 

} 

/* INT_Timer0 initiate 1.0 */ 

void Init_INT_Timer(void) 

{ 

TSCR2 =0x07; /* 128Hz at 16M bus clok */ 

/* 128Hz * 2/3 at 24m bus clock */ 

/* in fact it is a little more than it. */ 

TIOS |=0x01; /* I/O select */ 

TIE |=0x01; /* Interrupt Enable */ 

TSCR1|=0x80; /* TSCR1_TEN=1 //Timer Enable */ 

} 

/* INT_Timer0 1.0 */ 

interrupt void INT_Timer0(void) 

{ 

SYSCLOCK++; 

CarMain(); 

TC0 = TCNT + 1874; /* 1875-1 :100Hz */ 

/* F = Fosc / (TC*128) */ 

TFLG1 |=0x01; /* clear interrupt flag */ 

} 

/* not finished EEPROM */ 

void EEPROM(void) 

{ 

ECLKDIV = 0x4F; 

while(!(ECLKDIV&0x80)) /* wheather */ 

{} 

while(!(ESTAT&0x80)) /* wheather the command buffer is empty */ 

{} 

while(!(EPROT&0x80)) /* wheather the eeprom is enabled to */ 

{} 

ECMD = 0x41; 

ESTAT |= 0x80; 

while(!(ESTAT&0x80)) /* wheather the command buffer is empty */ 

{} 

} 

/* 

*********************************************************************************** 

* PID.c 

* Description: This file includes some basic calculation function of PID 

* (c) Copyright 2006,Zhao Cheng 

* All Rights Reserved 

* 

* By : Zhao Cheng 

* Data : 2006_5_6 

* Note : Don't change this file if possible. 

**********************************************************************************/ 

#include <mc9s12dg128.h> /* derivative information */ 

/* 

*********************************************************************************** 

* 宏定義 

**********************************************************************************/ 

#define STABMAX 50 

#define SENSORNUM 8 

#define SAMPLETIMES 5 

/* 

*********************************************************************************** 

* FUNCTION PROTOTYPES 

**********************************************************************************/ 

int CalculateP(void); 

float CalculatePID(void); 

/********************************** PID控制程序 ********************************/ 

struct CARSTATE 

{ 

int E0; 

int E1; 

int E2; 

int E3; 

float Integral; 

}CarState; 

/* 

*********************************************************************************** 

* 初始化PID參數(shù) 

**********************************************************************************/ 

void Init_PID() 

{ 

CarState.E0 = 0; 

CarState.E1 = 0; 

CarState.E2 = 0; 

CarState.E3 = 0; 

CarState.Integral = 0; 

} 

/* 

********************************************************************************** 

* 信號處理函數(shù) 

* 

* 程序描述: 對傳感器采集過來的數(shù)據(jù)進行處理，得到一些基本的計算參數(shù) 

* 

* 說明： 無 

**********************************************************************************/ 

int SignalProcess( unsigned int signal ) 

{ 

const int BitValue[8] = {43,26,12,6,-6,-12,-26,-43}; //MAX:28 

int i,CurrPoint=0,LastPoint=0,BitNum=0; 

unsigned char SignalBit[8]; 

for(i=0;i<8;i++) 

{ 

SignalBit[i] = signal & 0x0001; 

BitNum += SignalBit[i]; 

signal >>= 1; 

} 

switch(BitNum) 

{ 

case 1: 

for(i=0;i<8;i++) 

if(SignalBit[i] != 0) 

CurrPoint += BitValue[i]; 

CarState.E0 = CurrPoint; 

break; 

case 2: 

for(i=0;i<8;i++) 

if(SignalBit[i] != 0) 

CurrPoint += BitValue[i]; 

CurrPoint >>= 1; 

CarState.E0 = CurrPoint; 

break; 

default: 

CarState.E0 = CarState.E1; 

break; 

} 

return CalculateP()*100; 

} 

/* 

********************************************************************************** 

* 

* PID計算函數(shù) 

* 

* 程序描述: 計算P參數(shù) 

* 

* 說明： 無 

**********************************************************************************/ 

int CalculateP(void) 

{ 

CarState.E1 = CarState.E0; 

return((int)CarState.E0); 

} 

/* 

*********************************************************************************** 

* PID計算函數(shù) 

* 

* 程序描述: 對傳感器采集過來的數(shù)據(jù)進行處理，得到一些基本的計算參數(shù) 

* 

* 說明： 無 

**********************************************************************************/ 

float CalculatePID(void) 

{ 

float P, I = 0, D; 

/* parameter const */ 

float Kp = 1.0, Ki = -0.0002, Kd = -0.0002; 

/* P parameter */ 

P = CarState.E0 * Kp; 

/* I parameter */ 

if(P+I<2) 

{ 

CarState.Integral += Ki * CarState.E0; 

I = CarState.Integral; 

} 

/* D parameter */ 

D = Kd * ( CarState.E0 + 3*CarState.E1 - 3*CarState.E2 - CarState.E3 )/6.0; 

CarState.E3 = CarState.E2; 

CarState.E2 = CarState.E1; 

CarState.E1 = CarState.E0; 

return (P+I+D); 

}