小車(chē)控制程序清單 


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

/* 

************************************************************************************ main.c 

* 

*

* 

* 

* 

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

#include <hidef.h> /* common defines and macros */ 

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

#pragma LINK_INFO DERIVATIVE "mc9s12dg128b" 

#define HIGHSPEED 11500 /* 速度參量，此處未使用測(cè)速模塊 */ 

#define LOWSPEED0 12500 /* 0-24000 數(shù)值越大，速度越慢 */ 

#define LOWSPEED1 12000 /* used in CarMain() */ 

#define STABMAX 50 

#define StopCar() PORTK |= 0x80 /* stop the motor */ 

#define StartCar() PORTK |= 0x04 /* start the motor */ 

#define BrakeCar() PORTK &= 0xfb /* slow the speed of the SmartCar */ 

unsigned int SYSCLOCK=0; /* update in INT_Timer0() */ 

/* 

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

* FUNCTION PROTOTYPES 

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

/* write in "SmartCar.c" */ 

void Init_INT_RTI(void); /* initiate Real Time Interrupt */ 

void Init_INT_Timer(void); /* INT_Timer0 initiate */ 

void Init_PWMout(void); /* initiate PWM output */ 

void PWMout(int, int); /* output PWM */ 

/* write in "PID.c" */ 

void Init_PID(void); /* initiate PID parameter */ 

int CalculateP(void); /* calculate parameter P */ 

float CalculatePID(void); /* calculate PID */ 

int SignalProcess(unsigned char); /* Process the signal from the sensors */ 

/* write in "Test.c" */ 

void IOtest(void); /* Test I/O */ 

void PWMtest(void); /* Test PWM output */ 

int SignalTest(void); /* Test the sensors */ 

/* write in local file */ 

void Init(void); /* initiate parameter */ 

void ProtectMoto(void); /* the function protecting the Motor */ 

void CarMain(void); /* SmartCar main function */ 

/* 

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

* 主程序 

* 

* 程序描述: 完成智能小車(chē)系統(tǒng)的初始化，通過(guò)按鍵可選擇工作模式，有I/O測(cè)試，PWM 輸出測(cè)試 

* 傳感器測(cè)試，以及小車(chē)正常工作模式 

* 

* 硬件連接：PORTB 接傳感器 

* PWM 輸出口 （1）接舵機(jī) （2）接電機(jī)驅(qū)動(dòng)芯片MC33886 

* 

* 說(shuō)明： 無(wú) 

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

void main(void) 

{ 

Init(); 

DDRB = 0x00; 

switch(PORTB) 

{ 

case 0x80: 

IOtest(); 

break; 

case 0x40: 

PWMtest(); 

break; 

case 0x20: 

SignalTest(); 

break; 

default: 

DDRA = 0x00; 

DDRB = 0xff; 

DDRK = 0xff; 

PORTB = 0xff; 

CarMain(); 

EnableInterrupts; 

for(;;); 

break; 

} 

} 

/* 

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

* 小車(chē)尋跡行駛函數(shù) 

* 

* 程序描述: 通過(guò)傳感器采集數(shù)據(jù)，并對(duì)其進(jìn)行處理，通過(guò)PID算法得出小車(chē)穩(wěn)定行駛所需的參數(shù)，進(jìn)而調(diào)用PWM輸出函數(shù) 

* 控制舵機(jī)與電機(jī)的工作 

* 

* 注意： 這個(gè)函數(shù)調(diào)用了 SignalProcess(unsigned char)，BrakeCar()，PWMout(Direction, Velocity) 

* 

* 說(shuō)明： 無(wú) 

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

void CarMain(void) 

{ 

static int Direction=0, Velocity; 

static unsigned char signal; 

static unsigned int BrakeTime = 0, BrakeControl = 0; 

static unsigned int Stability=0, Stab[STABMAX], PStab=0, StabAver; 

int i; 

signal = PORTA; 

PORTB = ~signal; 

Direction = SignalProcess( signal ); 

/* 穩(wěn)定性系數(shù)的計(jì)算 */ 

Stability -= Stab[PStab]; 

Stab[PStab] = (unsigned int)Direction/100; 

Stability += Stab[PStab]; 

PStab++; 

if(PStab >= STABMAX) PStab=0; 

StabAver = 0; 

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

{ 

if(Stability > Stab[i]) 

StabAver += Stability - Stab[i]; 

else 

StabAver += Stab[i] - Stability; 

} 

if( BrakeTime != 0) 

{ 

BrakeTime--; 

BrakeCar(); 

} 

else 

{ 

StartCar(); 

if(BrakeControl>0) 

BrakeControl--; 

if(Direction < -4000 || Direction > 4000 ) 

{ 

Velocity = LOWSPEED0; 

if(BrakeControl == 0 && StabAver/STABMAX<22) 

{ 

BrakeTime = 20; 

BrakeControl = 120; 

} 

} 

else 

{ 

if(Direction < -2500 || Direction > 2500 ) 

Velocity = LOWSPEED1; 

else 

Velocity = HIGHSPEED; 

} 

} 

PWMout(Direction, Velocity); 

} 

/* 

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

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

* 

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

* 

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

* 

* 說(shuō)明： 無(wú) 

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

void Init(void) 

{ 

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

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

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

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

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)時(shí)鐘 */ 

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; /* 設(shè)置舵機(jī)角度 */ 

if(Velocity>24000) Velocity=24000; 

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

PWMDTY45 = Velocity; /* 設(shè)置電機(jī)速度 */ 

} 

/* 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;