?? main.c
字號:
/*
*********************************************************************************
*
* 工程名稱：SmartCar
*
* 功能描述：結(jié)合飛思卡爾16位單片機(jī)MC9S12DG128B完成小車自動尋跡，沿黑線行駛功能
*
* IDE環(huán)境： Metrowerks CodeWarrior 4.1
*
* 組成文件： main.c
*
* SmartCar.c/PID.c/LCD1620.c/Test.c
*
* 說明： 本版本為智能小車程序早期版本，還有待更進(jìn)一步完善
*
* 日期： 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 數(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 */
void 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 */
/* ***********************************************************************************
*
*
* 主程序
*
* 程序描述: 完成智能小車系統(tǒng)的初始化，通過按鍵可選擇工作模式，有I/O測試，PWM 輸出測試
*
* 傳感器測試，以及小車正常工作模式
*
* 硬件連接: PORTB 接傳感器 ;
*
* PWM 輸出口 （1）接舵機(jī) （2）接電機(jī)驅(qū)動芯片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;
}
}
/*
***********************************************************************************
*
*
* 小車尋跡行駛函數(shù)
*
*
* 程序描述: 通過傳感器采集數(shù)據(jù)，并對其進(jìn)行處理，通過PID算法得出小車穩(wěn)定行駛所需的參數(shù),
*
* 而調(diào)用PWM輸出函數(shù) 控制舵機(jī)與電機(jī)的工作
*
* 注意： 這個函數(shù)調(diào)用了 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 ); /* 穩(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)時鐘，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 unsigned int 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; /* 設(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 SENSORNUM 8
#define SAMPLETIMES 5
/* *********************************************************************************
*
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -