华中科技小车控制程序清单.docx

华中科技小车控制程序清单.docx

《华中科技小车控制程序清单.docx》由会员分享，可在线阅读，更多相关《华中科技小车控制程序清单.docx（22页珍藏版）》请在冰豆网上搜索。

华中科技小车控制程序清单

/*

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

*工程名称：

SmartCar

*功能描述：

结合飞思卡尔16位单片机MC9S12DG128B完成小车自动寻迹，沿黑线行驶功能

*IDE环境：

MetrowerksCodeWarrior4.1

*组成文件：

*main.c

*SmartCar.c/PID.c/LCD1620.c/Test.c

*说明：

本版本为智能小车程序早期版本，还有待更进一步完善

*日期：

ffice:

smarttags"/>2006-5-6

*（c）Copyright2006,ZhaoCheng

*AllRightsReserved

*

*

*By:

ZhaoCheng

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

/*

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

*

*（c）Copyright2006,ZhaoCheng

*AllRightsReserved

*

*By:

ZhaoCheng

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

#include/*commondefinesandmacros*/

#include/*derivativeinformation*/

#pragmaLINK_INFODERIVATIVE"mc9s12dg128b"

#defineHIGHSPEED11500/*速度参量，此处未使用测速模块*/

#defineLOWSPEED012500/*0-24000数值越大，速度越慢*/

#defineLOWSPEED112000/*usedinCarMain（）*/

#defineSTABMAX50

#defineStopCar（）PORTK|=0x80/*stopthemotor*/

#defineStartCar（）PORTK|=0x04/*startthemotor*/

#defineBrakeCar（）PORTK&=0xfb/*slowthespeedoftheSmartCar*/

unsignedintSYSCLOCK=0;/*updateinINT_Timer0（）*/

/*

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

*FUNCTIONPROTOTYPES

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

/*writein"SmartCar.c"*/

voidInit_INT_RTI（void）;/*initiateRealTimeInterrupt*/

voidInit_INT_Timer（void）;/*INT_Timer0initiate*/

voidInit_PWMout（void）;/*initiatePWMoutput*/

voidPWMout（int,int）;/*outputPWM*/

/*writein"PID.c"*/

voidInit_PID（void）;/*initiatePIDparameter*/

intCalculateP（void）;/*calculateparameterP*/

floatCalculatePID（void）;/*calculatePID*/

intSignalProcess（unsignedchar）;/*Processthesignalfromthesensors*/

/*writein"Test.c"*/

voidIOtest（void）;/*TestI/O*/

voidPWMtest（void）;/*TestPWMoutput*/

intSignalTest（void）;/*Testthesensors*/

/*writeinlocalfile*/

voidInit（void）;/*initiateparameter*/

voidProtectMoto（void）;/*thefunctionprotectingtheMotor*/

voidCarMain（void）;/*SmartCarmainfunction*/

/*

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

*主程序

*

*程序描述:

完成智能小车系统的初始化，通过按键可选择工作模式，有I/O测试，PWM输出测试

*传感器测试，以及小车正常工作模式

*

*硬件连接：

PORTB接传感器

*PWM输出口

（1）接舵机

（2）接电机驱动芯片MC33886

*

*说明：

无

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

voidmain（void）

{

Init（）;

DDRB=0x00;

switch（PORTB）

{

case0x80:

IOtest（）;

break;

case0x40:

PWMtest（）;

break;

case0x20:

SignalTest（）;

break;

default:

DDRA=0x00;

DDRB=0xff;

DDRK=0xff;

PORTB=0xff;

CarMain（）;

EnableInterrupts;

for（;;）;

break;

}

}

/*ffice

ffice"/>

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

*小车寻迹行驶函数

*

*程序描述:

通过传感器采集数据，并对其进行处理，通过PID算法得出小车稳定行驶所需的参数，进而调用PWM输出函数

*控制舵机与电机的工作

*

*注意：

这个函数调用了SignalProcess（unsignedchar），BrakeCar（），PWMout（Direction,Velocity）

*

*说明：

无

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

voidCarMain（void）

{

staticintDirection=0,Velocity;

staticunsignedcharsignal;

staticunsignedintBrakeTime=0,BrakeControl=0;

staticunsignedintStability=0,Stab[STABMAX],PStab=0,StabAver;

inti;

signal=PORTA;

PORTB=~signal;

Direction=SignalProcess（signal）;

/*稳定性系数的计算*/

Stability-=Stab[PStab];

Stab[PStab]=（unsignedint）Direction/100;

Stability+=Stab[PStab];

PStab++;

if（PStab>=STABMAX）PStab=0;

StabAver=0;

for（i=0;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）;

}

/*

***********************************************************************************ffice

ffice"/>

*系统初始化函数

*

*程序描述:

初始化了系统时钟，FLASH和EEPRO的工作频率，PWM输出口，定时器，以及PID算法中的有关参数

*

*注意：

这个函数调用了Init_PWMout（）nit_INT_Timer（）nit_PID（）　　

*

*说明：

无

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

voidInit（void）

{

REFDV=0x01;/*initiatePLLclock*/

SYNR=0x02;/*systemclockffice:

smarttags"/>24M*/

while（!

（CRGFLG&0x08））{}/*waituntillsteady*/

CLKSEL=0x80;/*选定所相环时钟*/

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

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

ECLKDIV=0x49;

Init_PWMout（）;/*01:

50Hz45:

1kHz*/

Init_INT_Timer（）;/*initiateETC（EnhancedCaptureClock）*/

Init_PID（）;/*initiatePIDcaculatingprocess*/

DDRK|=0x80;/*StartCar--stopcar*/

PORTK&=0x7F;

}

/*

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

*SmartCar.c

*

*（c）Copyright2006,ZhaoCheng

*AllRightsReserved

*

*By:

ZhaoCheng

*Data:

2006_5_6

*Note:

Don'tchangethisfileifpossible.

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

#include

#include

externSYSCLOCK;/*引用全局变量,系统时钟*/

voidCarMain（void）;

/*

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

*PWM初始化函数

*

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

voidInit_PWMout（void）

{

PWME=0x22;/*01:

50Hz45:

1kHz*/

PWMPOL=0x22;

PWMCTL=0x50;

PWMCLK=0x02;

PWMSCLA=4;

}

/*

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

*PWM输出函数

*程序描述：

输入参数为方向，速度

*方向：

－45～45

*速度：

0～24000

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

voidPWMout（intDirection,intVelocity）

{

Direction=Direction/3+4500;

if（Direction<3000）Direction=3000;

if（Direction>6000）Direction=6000;

PWMPER01=60000;/*Center1500ms*3*/

PWMDTY01=Direction+93;/*设置舵机角度*/

if（Velocity>24000）Velocity=24000;

PWMPER45=24000;/*1kHz（<10kHz）*/

PWMDTY45=Velocity;/*设置电机速度*/

}

/*initiateRealTimeInterrupt1.0*/

voidInit_INT_RTI（void）

{

RTICTL=0x74;

CRGINT|=0x80;

}

/*RealTimeInterrupt1.0*/

interruptvoidINT_RTI（void）

{

CRGFLG|=0x80;/*cleartheinterrruptflag*/

}

/*INT_Timer0initiate1.0*/

voidInit_INT_Timer（void）

{

TSCR2=0x07;/*128Hzat16Mbusclok*/

/*128Hz*2/3at24mbusclock*/

/*infactitisalittlemorethanit.*/

TIOS|=0x01;/*I/Oselect*/

TIE|=0x01;/*InterruptEnable*/

TSCR1|=0x80;/*TSCR1_TEN=1//TimerEnable*/

}

/*INT_Timer01.0*/

interruptvoidINT_Timer0（void）

{

SYSCLOCK++;

CarMain（）;

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

100Hz*/

/*F=Fosc/（TC*128）*/

TFLG1|=0x01;/*clearinterruptflag*/

}

/*notfinishedEEPROM*/

voidEEPROM（void）

{

ECLKDIV=0x4F;

while（!

（ECLKDIV&0x80））/*wheather*/

{}

while（!

（ESTAT&0x80））/*wheatherthecommandbufferisempty*/

{}

while（!

（EPROT&0x80））/*wheathertheeepromisenabledto*/

{}

ECMD=0x41;

ESTAT|=0x80;

while（!

（ESTAT&0x80））/*wheatherthecommandbufferisempty*/

{}

}

/*ffice

ffice"/>

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

*PID.c

*Description:

ThisfileincludessomebasiccalculationfunctionofPID

*（c）Copyright2006,ZhaoCheng

*AllRightsReserved

*

*By:

ZhaoCheng

*Data:

2006_5_6

*Note:

Don'tchangethisfileifpossible.

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

#include/*derivativeinformation*/

/*

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

*宏定义

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

#defineSTABMAX50

#defineSENSORNUM8

#defineSAMPLETIMES5

/*

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

*FUNCTIONPROTOTYPES

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

intCalculateP（void）;

floatCalculatePID（void）;

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

structCARSTATE

{

intE0;

intE1;

intE2;

intE3;

floatIntegral;

}CarState;

/*

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

*初始化PID参数

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

voidInit_PID（）

{

CarState.E0=0;

CarState.E1=0;

CarState.E2=0;

CarState.E3=0;

CarState.Integral=0;

}

/*

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

*信号处理函数

*

*程序描述:

对传感器采集过来的数据进行处理，得到一些基本的计算参数

*

*说明：

无

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

intSignalProcess（unsignedintsignal）

{

constintBitValue[8]={43,26,12,6,-6,-12,-26,-43};//MAX:

28

inti,CurrPoint=0,LastPoint=0,BitNum=0;

unsignedcharSignalBit[8];

for（i=0;i<8;i++）

{

SignalBit[i]=signal&0x0001;

BitNum+=SignalBit[i];

signal>>=1;

}

switch（BitNum）

{

case1:

for（i=0;i<8;i++）

if（SignalBit[i]!

=0）

CurrPoint+=BitValue[i];

CarState.E0=CurrPoint;

break;

case2:

for（i=0;i<8;i++）

if（SignalBit[i]!

=0）