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

1、华中科技小车控制程序清单/* * * 工程名称：SmartCar * 功能描述：结合飞思卡尔16位单片机MC9S12DG128B完成小车自动寻迹，沿黑线行驶功能 * IDE环境： Metrowerks CodeWarrior 4.1 * 组成文件： * main.c * SmartCar.c/PID.c/LCD1620.c/Test.c * 说明： 本版本为智能小车程序早期版本，还有待更进一步完善 * 日期：ffice:smarttags /2006-5-6 * (c) Copyright 2006,Zhao Cheng * All Rights Reserved * * * By : Zha

2、o Cheng */ /* * main.c * * (c) Copyright 2006,Zhao Cheng * All Rights Reserved * * By : Zhao Cheng */ #include /* common defines and macros */ #include /* derivative information */ #pragma LINK_INFO DERIVATIVE mc9s12dg128b #define HIGHSPEED 11500 /* 速度参量，此处未使用测速模块 */ #define LOWSPEED0 12500 /* 0-240

3、00 数值越大，速度越慢 */ #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 I

4、NT_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

5、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

6、 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 */ /* * * 主程序 * * 程序描述: 完成智能小车系统的初始化，通过按键可选择工作模式，有I/O测试，PWM 输出

7、测试 * 传感器测试，以及小车正常工作模式 * * 硬件连接：PORTB 接传感器 * PWM 输出口 （1）接舵机 （2）接电机驱动芯片MC33886 * * 说明： 无 */ 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(

8、); EnableInterrupts; for(;); break; /* fficeffice /* * 小车寻迹行驶函数 * * 程序描述: 通过传感器采集数据，并对其进行处理，通过PID算法得出小车稳定行驶所需的参数，进而调用PWM输出函数 * 控制舵机与电机的工作 * * 注意： 这个函数调用了 SignalProcess(unsigned char)，BrakeCar()，PWMout(Direction, Velocity) * * 说明： 无 */ void CarMain(void) static int Direction=0, Velocity; static unsig

9、ned char signal; static unsigned int BrakeTime = 0, BrakeControl = 0; static unsigned int Stability=0, StabSTABMAX, PStab=0, StabAver; int i; signal = PORTA; PORTB = signal; Direction = SignalProcess( signal ); /* 稳定性系数的计算 */ Stability -= StabPStab; StabPStab = (unsigned int)Direction/100; Stability

10、 += StabPStab; PStab+; if(PStab = STABMAX) PStab=0; StabAver = 0; for(i=0;i Stabi) StabAver += Stability - Stabi; else StabAver += Stabi - Stability; if( BrakeTime != 0) BrakeTime-; BrakeCar(); else StartCar(); if(BrakeControl0) BrakeControl-; if(Direction 4000 ) Velocity = LOWSPEED0; if(BrakeContro

11、l = 0 & StabAver/STABMAX22) BrakeTime = 20; BrakeControl = 120; else if(Direction 2500 ) Velocity = LOWSPEED1; else Velocity = HIGHSPEED; PWMout(Direction, Velocity); /* * fficeffice /* 系统初始化函数 * * 程序描述: 初始化了系统时钟，FLASH 和 EEPRO的工作频率，PWM输出口，定时器，以及PID算法中的有关参数 * * 注意： 这个函数调用了 Init_PWMout()nit_INT_Timer(

12、)nit_PID() * * 说明： 无 */ void Init(void) REFDV=0x01; /* initiate PLL clock */ SYNR =0x02; /* system clock ffice:smarttags /24M */ while (!(CRGFLG & 0x08) /* wait untill steady */ CLKSEL=0x80; /* 选定所相环时钟 */ FCLKDIV=0x49; /* 使FLASH 和 EEPROM */ /* 的擦除操作工作频率在200HZ左右 */ ECLKDIV=0x49; Init_PWMout(); /* 01:

13、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 * Not

14、e : Dont change this file if possible. */ #include #include extern SYSCLOCK; /* 引用全局变量,系统时钟 */ void CarMain(void); /* * * PWM初始化函数 * */ void Init_PWMout(void) PWME = 0x22; /*01:50Hz 45:1kHz */ PWMPOL = 0x22; PWMCTL = 0x50; PWMCLK = 0x02; PWMSCLA = 4; /* * * PWM输出函数 * 程序描述：输入参数为方向，速度 * 方向：4545 * 速度：0

15、24000 */ void PWMout(int Direction, int Velocity) Direction = Direction/3 + 4500; if(Direction6000) Direction=6000; PWMPER01 = 60000; /* Center 1500ms*3 */ PWMDTY01 = Direction+93; /* 设置舵机角度 */ if(Velocity24000) Velocity=24000; PWMPER45 = 24000; /* 1kHz ( * * PID.c * Description: This file includes

16、some basic calculation function of PID * (c) Copyright 2006,Zhao Cheng * All Rights Reserved * * By : Zhao Cheng * Data : 2006_5_6 * Note : Dont change this file if possible. */ #include /* derivative information */ /* * * 宏定义 */ #define STABMAX 50 #define SENSORNUM 8 #define SAMPLETIMES 5 /* * * FU

17、NCTION PROTOTYPES */ int CalculateP(void); float CalculatePID(void); /* PID控制程序 */ struct CARSTATE int E0; int E1; int E2; int E3; float Integral; CarState; /* * * 初始化PID参数 */ void Init_PID() CarState.E0 = 0; CarState.E1 = 0; CarState.E2 = 0; CarState.E3 = 0; CarState.Integral = 0; /* * * 信号处理函数 * *

18、 程序描述: 对传感器采集过来的数据进行处理，得到一些基本的计算参数 * * 说明： 无 */ int SignalProcess( unsigned int signal ) const int BitValue8 = 43,26,12,6,-6,-12,-26,-43; /MAX:28 int i,CurrPoint=0,LastPoint=0,BitNum=0; unsigned char SignalBit8; for(i=0;i= 1; switch(BitNum) case 1: for(i=0;i8;i+) if(SignalBiti != 0) CurrPoint += BitValuei; CarState.E0 = CurrPoint; break; case 2: for(i=0;i8;i+) if(SignalBiti != 0)