?? freescale智能車大賽源程序.c
字號:
小車控制程序清單
/*
***********************************************************************************
* 工程名稱：SmartCar
* 功能描述：結合飛思卡爾16位單片機MC9S12DG128B完成小車自動尋跡，沿黑線行駛功能
* IDE環境： Metrowerks CodeWarrior 4.1
* 組成文件：
* main.c
* SmartCar.c/PID.c/LCD1620.c/Test.c
* 說明： 本版本為智能小車程序早期版本，還有待更進一步完善
* 日期：2006-5-6
* (c) Copyright 2006,Zhao Cheng
* All Rights Reserved
*
*
* By : Zhao Cheng
**********************************************************************************/
/*
************************************************************************************ main.c
*
* (c) Copyright 2006,Zhao Cheng
* All Rights Reserved
*
* By : Zhao Cheng
**********************************************************************************/
#include <hidef.h> /* common defines and macros */
#include <mc9s12dg128.h> /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
#define HIGHSPEED 11500 /* 速度參量，此處未使用測速模塊 */
#define LOWSPEED0 12500 /* 0-24000 數值越大，速度越慢 */
#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 */
/*
***********************************************************************************
* 主程序
*
* 程序描述: 完成智能小車系統的初始化，通過按鍵可選擇工作模式，有I/O測試，PWM 輸出測試
* 傳感器測試，以及小車正常工作模式
*
* 硬件連接：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();
EnableInterrupts;
for(;;);
break;
}
}
/*
***********************************************************************************
* 小車尋跡行駛函數
*
* 程序描述: 通過傳感器采集數據，并對其進行處理，通過PID算法得出小車穩定行駛所需的參數，進而調用PWM輸出函數
* 控制舵機與電機的工作
*
* 注意： 這個函數調用了 SignalProcess(unsigned char)，BrakeCar()，PWMout(Direction, Velocity)
*
* 說明： 無
**********************************************************************************/
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 );
/* 穩定性系數的計算 */
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);
}
/***********************************************************************************
* 系統初始化函數
*
* 程序描述: 初始化了系統時鐘，FLASH 和 EEPRO的工作頻率，PWM輸出口，定時器，以及PID算法中的有關參數
*
* 注意： 這個函數調用了 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; /* 選定所相環時鐘 */
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; /* 引用全局變量,系統時鐘 */
void CarMain(void);
/*
***********************************************************************************
* PWM初始化函數
*
**********************************************************************************/
void Init_PWMout(void)
{
PWME = 0x22; /*01:50Hz 45:1kHz */
PWMPOL = 0x22;
PWMCTL = 0x50;
PWMCLK = 0x02;
PWMSCLA = 4;
}
/*
***********************************************************************************
* PWM輸出函數
* 程序描述：輸入參數為方向，速度
* 方向：－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;
/*
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -