飞思卡尔智能车dg128单片机控制程序代码介绍
时间：10-02
整理：3721RD
点击：
以下是飞思卡尔智能车dg128单片机控制程序代码介绍：
#include <hidef.h> /* common defines and macros */
#include <mc9s12dg128.h> /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
#pragma CODE_SEG __NEAR_SEG NON_BANKED
//定义全局变量
int blackline=500;
unsigned int Get_pulse;
int qian[11]; //AD转换结果
int hou[3];
int dw_result[4]; //档位信号
int dangwei;
int pianxin_a[11]={115,85,62,41,20,0,-20,-41,-62,-85,-115}; //两排灯距离中线的偏移量
int go=0;
int qidian,once;
int speed; //要求速度
int counter_a,counter_1,counter_2,counter_3,counter_b; //前后两排在黑线阀值上的灯数
int one,two,three; int x; //前后排灯在黑线上的灯编号
int biga,bigb,bigc; int bigd; //前排最大的三个电压值he后排的最大值
int tan_o,tan_n1,tan_n2,tan_n3,tan_n4,tan_n5,tan_n6;
int tan_n; //前，后次偏角正弦
int away_o,away_n; //前后次车身中线偏移量
int kj;int kp;int zhuan,o; //控制舵机的参数
int jia_1,jia_2,jia_3; //速度参数
int sudu; //真实速度
//函数声明
void system_init(void); //模块初始化函数
void clock_init(void);
void PWM_init(void);
void AD_Init(void);
void set_parameter(void); //键盘输入参数显示
void Infrared_detect(void); //红外检测速
void data_handle(void);
void direct_ctl(void); //方向控制
void steer_ctl(void); //舵机控制
void slow1(void);
void slow2(void);
void slow3(void);
void slow4(void);
void slow5(void);
void slow6(void);
//刹车程序
void speed_ctl(void); //速度控制
void motor_ctl(void); //电机控制
void PACBInit(void);
void MDCInit(void);
//++++++++++++++++++++++++++++++++++++++++++++
//主函数开始
void main(void)
{
DisableInterrupts;
system_init(); //系统初始化
set_parameter();
PACBInit();
MDCInit();
EnableInterrupts;
for(;;)
{
Infrared_detect();
data_handle();
motor_ctl();
steer_ctl();
}
}
void interrupt 26 MDC_ISR(void)
{
static unsigned int number_count=0; static unsigned int start=0; static
unsigned int begin=0;
MCFLG = 0x80;
{ number_count ++;
if( number_count==125) //0.125秒取一次
{
number_count=0;
Get_pulse = PACN10; //get pluse 脉冲数
PACN10 = 0X0000;
}
}
{
start++;
if(start>100) //3sec
{begin++; start=0;
if(begin>22)
go=1;
if(begin<150&&begin>22)
go=2;
if(begin>=150)
go=3;
}
}
}
//-----系统初始化-----------------------
void system_init(void) //system initiat
{DDRB=0xff;
clock_init(); //clock initiat
PWM_init(); //pwm initiat
AD_Init(); //ATD initiat
}
void clock_init(void) //时钟初始化
{
CLKSEL=0X00; //disengage PLL to system
PLLCTL_PLLON=1; //turn on PLL
SYNR=2;
REFDV=1; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=24MHz;
_asm(nop); //BUS CLOCK=16M
_asm(nop);
while(！(CRGFLG_LOCK==1)); //when pll is steady ,then use it;
CLKSEL_PLLSEL =1; //engage PLL to system;
}
void AD_Init(void)
{
ATD0CTL2=0xC0; //AD模块上电, 快速清零, 无等待模式, 禁止外部触发, 中断禁止
ATD0CTL3=0x44; //每次转换8个序列, FIFO, Freeze模式下继续转
ATD0CTL4=0x02; //10位精度, 采样时间为2个AD时钟周期,ATDClock=4MHz
ATD0CTL5=0xB0; //右对齐无符号，,连续转换 ,8轮流通道采样
ATD0DIEN=0x00; //禁止数字输入
ATD1CTL2=0xC0; //AD模块上电, 快速清零, 无等待模式, 禁止外部触发, 中断禁止
ATD1CTL3=0x44; //每次转换8个序列, FIFO, Freeze模式下继续转
ATD1CTL4=0x02; //10位精度, 采样时间为2个AD时钟周期,ATDClock=4MHz
ATD1CTL5=0xB0; //右对齐无符号，,连续转换 ,8轮流通道采样
ATD1DIEN=0x00; //禁止数字输入
}
void PWM_init(void)
{
PWME =0x00; //关闭
PWMPOL=0xff; //先高电平
PWMCAE=0x00; //左对齐
PWMCLK=0x0c; //选择0，1，4，5通道时钟源头A 2，3通道时钟源头SB,6,7为时钟B
PWMPRCLK=0x33; //时钟预分频0通道为8
//设置舵机
PWMCTL_CON01=1; //使得通道0，1成为16位pwm
PWMPER0 =0x75;
PWMPER1 =0x30; //舵机的频率是： 24M/8/30000=100Hz,T=10ms
PWMDTY01=4500; // 对应为4500/30000的占空比,待调整
//设置电机
PWMSCLB =50; // 24MHZ/8/2/50=300K
PWMPER2 =100; //电机的频率是： 24M/8/2/50/500=300Hz,T=5ms
PWMDTY2 =0; //用于测试电机
PWMPER3 =100; //电机的频率是： 24M/8/2/50/500=300Hz,T=5ms
PWMDTY3 =0; //用于测试电机
//小灯输出
PWMPER4 =79; //小灯的频率是： 24M/8/79=38kHz
PWMDTY4 =30;
PWMPER7 =79; //小灯的频率是： 24M/8/79=38kHz
PWMDTY7 =30;
DDRP =0xff; //控制输出
PWME =0x9f; //开放通道0，1 ,3,4,5,6,7
}
void PACBInit()
{
TCTL4 = 0X02; //下降沿捕捉脉冲
PBCTL = 0x40; //级联两个8位累加器(PAC0和PAC1)
ICPAR = 0X03; //使能累加器
PACN10 = 0X0000;
}
void MDCInit(void)
{
MCCTL = MCCTL&0Xfb; //模数计数器禁止运行
MCCTL = 0Xe3; //允许中断，模数计数方式
//返回时重新加载所用的常数，分频常数为16
MCCTL = MCCTL|0X04; //模数计数器使能
MCCNT = 1500; //(1/24M)*16*1500= 1ms
MCCTL = MCCTL|0X08 ; //把模数常数寄存器的值加载到模数计数器FLMC;
}
void set_parameter(void)//----------参数设置----------
{
blackline=500;
tan_n=0;
qidian=0,once=0;
speed=0;
counter_a=0,counter_b=0; //前后两排在黑线阀值上的灯数
one=5;two=4;three=6;x=1; //前后排灯在黑线上的灯编号
biga=0;bigb=0;bigc=0; bigd=0; //前排最大的三个电压值he后排的最大值
tan_o=0,tan_n=0; //前，后次偏角正弦
tan_n1=0;tan_n2=0;tan_n3=0;tan_n4=0;tan_n5=0;
away_o=0,away_n=0; //前后次车身中线偏移量
kj=0;kp=0;
zhuan=0;
}
//----------红外检测---------
void Infrared_detect(void)
{
while(！ATD0STAT1_CCF0);
qian[0] = ATD0DR0;
while(！ATD0STAT1_CCF1);
qian[1] = ATD0DR1;
while(！ATD0STAT1_CCF2);
qian[2] = ATD0DR2;
while(！ATD1STAT1_CCF4);
qian[3] = ATD1DR4;
while(！ATD0STAT1_CCF3);
qian[4] = ATD0DR3;
while(！ATD0STAT1_CCF4);
qian[5] = ATD0DR4;
while(！ATD0STAT1_CCF5);
qian[6] = ATD0DR5;
while(！ATD1STAT1_CCF5);
qian[7] = ATD1DR5;
while(！ATD0STAT1_CCF6);
qian[8] = ATD0DR6;
while(！ATD0STAT1_CCF7);
qian[9] = ATD0DR7;
while(！ATD1STAT1_CCF0);
qian[10] = ATD1DR0;
while(！ATD1STAT1_CCF1);
hou[0] = ATD1DR1;
while(！ATD1STAT1_CCF2);
hou[1] = ATD1DR2;
while(！ATD1STAT1_CCF3); //AD0所有和AD1前4个用作探测信号接受
hou[2] = ATD1DR3;
while(！ATD1STAT1_CCF6); //档位通道开始
dw_result[0] = ATD1DR6;
while(！ATD1STAT1_CCF7);
dw_result[1] = ATD1DR7;
//档位通道结束
}
void data_handle(void) //数据处理
{
int n;
counter_a=0,counter_b=0;
for(n=0;n<=10;n++)
{
if(qian[n]>blackline)
counter_a++;
}
for(n=0;n<=2;n++)
{
if(hou[n]>blackline)
counter_b++;
}
dangwei=0;
for(n=0;n<3;n++)
{
if(dw_result[n]>600) dangwei++; // 统计档位数目
}
switch( one ) //for testing
{
case 10: PORTB=0xf0;break;
case 9: PORTB=0xfe;break;
case 8: PORTB=0xfd;break;
case 7: PORTB=0xfb;break;
case 6: PORTB=0xf7;break;
case 5: PORTB=0xaa;break;
case 4: PORTB=0xef;break;
case 3: PORTB=0xdf;break;
case 2: PORTB=0xbf;break;
case 1: PORTB=0x7f;break;
case 0: PORTB=0x0f;break;
default:break;
}
{//判断起始点，参数待调整
if(((qian[1]>blackline&&qian[9]>blackline&&qian[2]>blackline
&&qian[10]>blackline)||(qian[1]>blackline&&qian[9]>blackline
&&qian[0]>blackline&&qian[8]>blackline))&&(qian[3]<blackline
||qian[4]<blackline||qian[6]<blackline||qian[7]<blackline)
&&(counter_a>4&&counter_a<8))
{
once++;tan_n=tan_o;away_n=away_o;
}
if(once>1&&counter_a<4)
{
if(go==2){qidian=1;once=0;}
if(go==3){qidian++;once=0;}
}
}
if(counter_a>5&&qian[4]>blackline&&qian[5]>blackline&&qian[6]>blackline&&
(qian[3]>blackline||qian[7]>blackline)) //过十字交叉线时或过起始线
{tan_n=tan_o; away_n=away_o;}
biga=0;
for(n=0;n<=10;n++) //寻找最大值
{if(biga<qian[n]){biga=qian[n];one=n;}}
bigb=0;
for(n=0;n<=10;n++) //寻找第二大值
{if((bigb<qian[n])&&n！=one){bigb=qian[n];two=n;}}
bigc=0;
for(n=0;n<=10;n++) //寻找第三大值
{if((bigc<qian[n])&&n！=one&&n！=two){bigc=qian[n];three=n;}}
bigd=0;
for(n=0;n<=2;n++)
{if(bigd<hou[n]){bigd=hou[n];x=n;}}
if(counter_a>5&&qian[4]>blackline&&qian[5]>blackline&&qian[6]>blackline)
one=5;
if((one-two)==1||(two-one)==1)
{
if(counter_a>0&&counter_a<3)
{
if( (biga-bigb)<=(bigb-bigc) ) tan_n=(pianxin_a[one]+pianxin_a[two])/2;
else tan_n=pianxin_a[one];
if(counter_b>0)
{
switch(x)
{case 1:
{
away_n=0; break;
}
case 0:
{
away_n=34; break;
}
case 2:
{
away_n=-34; break;
}
}
}
if(counter_b==0)
{
if(tan_n<=20&&tan_n>=-20)away_n=0;
if(tan_n>20)away_n=17;
if(tan_n<-20)away_n=-17;
}
}
if(counter_a==0)
{
if(tan_o<-100)
tan_n=-120;
if(tan_o>100)
tan_n=120;
if(tan_o<100&&tan_o>-100)
tan_n=tan_o;
away_n=away_o;
}
}
tan_n1=tan_n2;tan_n2=tan_n3;tan_n3=tan_n4;tan_n4=tan_n5;tan_n5=tan_n6;tan_n6=tan_n;
tan_n=(tan_n4+tan_n5+tan_n6+tan_n3+tan_n2+tan_n1)/6;
if(tan_o<50&&tan_n>-50){
if(tan_n-tan_o<-10||tan_n-tan_o>10) {
tan_n=tan_o;
if((tan_n5-tan_n4>5&&tan_n4-tan_n3>4&&tan_n3-tan_n2>3)||(tan_n5-tan_n4<-5&&tan_n4-
tan_n3<-4&&tan_n3-tan_n2<-3)
||(tan_n5-tan_n4>0&&tan_n4-tan_n3>0&&tan_n3-tan_n2>0&&tan_n2-tan_n1>0)
||(tan_n5-tan_n4<0&&tan_n4-tan_n3<0&&tan_n3-tan_n2<0&&tan_n2-tan_n1<0))
tan_n=tan_n6;
}
}
if(tan_o>50||tan_n<-50){
if(tan_n-tan_o<-15||tan_n-tan_o>15) {
tan_n=tan_o;
if((tan_n5-tan_n4>5&&tan_n4-tan_n3>4&&tan_n3-tan_n2>3)||(tan_n5-tan_n4<-5&&tan_n4-
tan_n3<-4&&tan_n3-tan_n2<-3)
||(tan_n5-tan_n4>0&&tan_n4-tan_n3>0&&tan_n3-tan_n2>0&&tan_n2-tan_n1>0)
||(tan_n5-tan_n4<0&&tan_n4-tan_n3<0&&tan_n3-tan_n2<0&&tan_n2-tan_n1<0))
tan_n=tan_n6;
}
}
tan_o=tan_n;away_o=away_n;
}
void direct_ctl(void)
{
if(tan_n>=-120&&tan_n<-90){kj=46;kp=12;}
if(tan_n>=-90&&tan_n<-60) {kj=47;kp=13;}
if(tan_n>=-60&&tan_n<-20) {kj=48;kp=14;}
if(tan_n>=-20&&tan_n<-8) {kj=46;kp=15;}
if(tan_n>=-8&&tan_n<0) {kj=45;kp=14;}
if(tan_n>=0&&tan_n<8) {kj=45;kp=14;}
if(tan_n>=8&&tan_n<20) {kj=46;kp=15;}
if(tan_n>=20&&tan_n<60) {kj=48;kp=14;}
if(tan_n>=60&&tan_n<90) {kj=47;kp=13;}
if(tan_n>=90&&tan_n<=120) {kj=47;kp=12;}
zhuan=(tan_n*kj+away_n*kp)/10;
}
void steer_ctl(void) //舵机控制
{
direct_ctl();
PWMDTY01=4620+zhuan;
}
//刹车函数参数待调整 j<?
void slow1(void)
{
int i;int j;
for(i=1;i<6;i++)
for(j=1;j<6;j++)
{
PWMDTY2=0;
PWMDTY3=10;}
PWMDTY3=0;
}
void slow2(void)
{
int i;int j;
for(i=1;i<8;i++)
for(j=1;j<7;j++)
{
PWMDTY2=0;
PWMDTY3=12;}
PWMDTY3=0;
}
void slow3(void)
{
int i;int j;
for(i=1;i<8;i++)
for(j=1;j<8;j++)
{ PWMDTY2=0;
PWMDTY3=14;}
PWMDTY3=0;
}
void slow4(void)
{
int i;int j;
for(i=1;i<10;i++)
for(j=1;j<15;j++)
{ PWMDTY2=0;
PWMDTY3=16;}
PWMDTY3=0;
jia_1++;}
void slow5(void)
{
int i;int j;
for(i=1;i<12;i++)
for(j=1;j<15;j++)
{ PWMDTY2=0;
PWMDTY3=18;}
PWMDTY3=0;
jia_2++;}
void slow6(void)
{
int i;int j;
for(i=1;i<15;i++)
for(j=1;j<13;j++)
{ PWMDTY2=0;
PWMDTY3=20;}
PWMDTY3=0;
jia_3++;}
void speed_ctl(void)
{
sudu=167*Get_pulse/10; //单位cm/s
if(zhuan<50&&zhuan>-50)speed=42;
if((zhuan<80&&zhuan>=50)||(zhuan<=-50&&zhuan>-80))speed=41;
if((zhuan<110&&zhuan>=80)||(zhuan<=-80&&zhuan>-110))speed=40;
if((zhuan<140&&zhuan>=110)||(zhuan<=-110&&zhuan>-140))speed=39;
if((zhuan<170&&zhuan>=140)||(zhuan<=-140&&zhuan>-170))speed=38;
if((zhuan<200&&zhuan>=170)||(zhuan<=-170&&zhuan>-200))speed=37;
if((zhuan<230&&zhuan>=200)||(zhuan<=-200&&zhuan>-230))speed=38;
if((zhuan<270&&zhuan>=230)||(zhuan<=-230&&zhuan>-270))speed=39;
if((zhuan<310&&zhuan>=270)||(zhuan<=-270&&zhuan>-310))speed=40;
if((zhuan<350&&zhuan>=310)||(zhuan<=-310&&zhuan>-350))speed=40;
if((zhuan<380&&zhuan>=350)||(zhuan<=-350&&zhuan>-380))speed=40;
if((zhuan<400&&zhuan>=380)||(zhuan<=-380&&zhuan>-400))speed=40;
if((zhuan<440&&zhuan>=400)||(zhuan<=-400&&zhuan>-440))speed=39;
if((zhuan<490&&zhuan>=440)||(zhuan<=-440&&zhuan>-490))speed=38;
if((zhuan<550&&zhuan>=490)||(zhuan<=-490&&zhuan>-550))speed=37;
if((zhuan<600&&zhuan>=550)||(zhuan<=-550&&zhuan>-600))speed=36;
if(zhuan>600||zhuan<-600)speed=35;
}
void motor_ctl(void)
{ int i; int t;
speed_ctl();
if(go>0)
{
if(qidian<2){
switch(dangwei){
case 0: {
if(zhuan>530||zhuan<-530)
{
if(sudu>230)slow3();
}
t=speed+25-sudu/10;
if(t>30&&t<70)
PWMDTY2=speed+25-sudu/10;break;
}
case 1: {
if(zhuan>300||zhuan<-300)
{
if(sudu>300)slow2();
if(sudu>330)slow3();
}
if((zhuan<=500&&zhuan>300)||(zhuan<-300&&zhuan>=-500))
{
if(sudu>280)slow4();
}
if(zhuan>500||zhuan<-500)
{
if(sudu>220)slow5();
}
if(jia_1+jia_2+jia_3>1)
{ for(i=0;i<500;i++) {
PWMDTY2= 75;
}
jia_1=0;jia_2=0;jia_3=0;
}
t=speed+35-sudu/10;
if(t>20&&t<70)
PWMDTY2=speed+38-sudu/12;break;
}
case 2: {
if(zhuan>280||zhuan<-280)
{
if(sudu>250)slow2();
if(sudu>290)slow3();
}
if((zhuan<=360&&zhuan>200)||(zhuan<-200&&zhuan>=-360))
{
if(sudu>240)slow4();
}
if((zhuan<=530&&zhuan>360)||(zhuan<-360&&zhuan>=-530))
{
if(sudu>220)slow5();
}
if(zhuan>520||zhuan<-520)
{
if(sudu>220)slow6();
}
if(jia_1+jia_2+jia_3>2)
{ for(i=0;i<600;i++) {
PWMDTY2= 80;
}
jia_1=0;jia_2=0;jia_3=0;
}
t=speed+70-sudu/9;
if(t>20&&t<90)
PWMDTY2=speed+70-sudu/9;break;
}
default: PWMDTY2=50;break;
}
}
else
{
PWMDTY2=0;
}
}
#include <hidef.h> /* common defines and macros */
#include <mc9s12dg128.h> /* derivative information */
#pragma LINK_INFO DERIVATIVE "mc9s12dg128b"
#pragma CODE_SEG __NEAR_SEG NON_BANKED
//定义全局变量
int blackline=500;
unsigned int Get_pulse;
int qian[11]; //AD转换结果
int hou[3];
int dw_result[4]; //档位信号
int dangwei;
int pianxin_a[11]={115,85,62,41,20,0,-20,-41,-62,-85,-115}; //两排灯距离中线的偏移量
int go=0;
int qidian,once;
int speed; //要求速度
int counter_a,counter_1,counter_2,counter_3,counter_b; //前后两排在黑线阀值上的灯数
int one,two,three; int x; //前后排灯在黑线上的灯编号
int biga,bigb,bigc; int bigd; //前排最大的三个电压值he后排的最大值
int tan_o,tan_n1,tan_n2,tan_n3,tan_n4,tan_n5,tan_n6;
int tan_n; //前，后次偏角正弦
int away_o,away_n; //前后次车身中线偏移量
int kj;int kp;int zhuan,o; //控制舵机的参数
int jia_1,jia_2,jia_3; //速度参数
int sudu; //真实速度
//函数声明
void system_init(void); //模块初始化函数
void clock_init(void);
void PWM_init(void);
void AD_Init(void);
void set_parameter(void); //键盘输入参数显示
void Infrared_detect(void); //红外检测速
void data_handle(void);
void direct_ctl(void); //方向控制
void steer_ctl(void); //舵机控制
void slow1(void);
void slow2(void);
void slow3(void);
void slow4(void);
void slow5(void);
void slow6(void);
//刹车程序
void speed_ctl(void); //速度控制
void motor_ctl(void); //电机控制
void PACBInit(void);
void MDCInit(void);
//++++++++++++++++++++++++++++++++++++++++++++
//主函数开始
void main(void)
{
DisableInterrupts;
system_init(); //系统初始化
set_parameter();
PACBInit();
MDCInit();
EnableInterrupts;
for(;;)
{
Infrared_detect();
data_handle();
motor_ctl();
steer_ctl();
}
}
void interrupt 26 MDC_ISR(void)
{
static unsigned int number_count=0; static unsigned int start=0; static
unsigned int begin=0;
MCFLG = 0x80;
{ number_count ++;
if( number_count==125) //0.125秒取一次
{
number_count=0;
Get_pulse = PACN10; //get pluse 脉冲数
PACN10 = 0X0000;
}
}
{
start++;
if(start>100) //3sec
{begin++; start=0;
if(begin>22)
go=1;
if(begin<150&&begin>22)
go=2;
if(begin>=150)
go=3;
}
}
}
//-----系统初始化-----------------------
void system_init(void) //system initiat
{DDRB=0xff;
clock_init(); //clock initiat
PWM_init(); //pwm initiat
AD_Init(); //ATD initiat
}
void clock_init(void) //时钟初始化
{
CLKSEL=0X00; //disengage PLL to system
PLLCTL_PLLON=1; //turn on PLL
SYNR=2;
REFDV=1; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=24MHz;
_asm(nop); //BUS CLOCK=16M
_asm(nop);
while(！(CRGFLG_LOCK==1)); //when pll is steady ,then use it;
CLKSEL_PLLSEL =1; //engage PLL to system;
}
void AD_Init(void)
{
ATD0CTL2=0xC0; //AD模块上电, 快速清零, 无等待模式, 禁止外部触发, 中断禁止
ATD0CTL3=0x44; //每次转换8个序列, FIFO, Freeze模式下继续转
ATD0CTL4=0x02; //10位精度, 采样时间为2个AD时钟周期,ATDClock=4MHz
ATD0CTL5=0xB0; //右对齐无符号，,连续转换 ,8轮流通道采样
ATD0DIEN=0x00; //禁止数字输入
ATD1CTL2=0xC0; //AD模块上电, 快速清零, 无等待模式, 禁止外部触发, 中断禁止
ATD1CTL3=0x44; //每次转换8个序列, FIFO, Freeze模式下继续转
ATD1CTL4=0x02; //10位精度, 采样时间为2个AD时钟周期,ATDClock=4MHz
ATD1CTL5=0xB0; //右对齐无符号，,连续转换 ,8轮流通道采样
ATD1DIEN=0x00; //禁止数字输入
}
void PWM_init(void)
{
PWME =0x00; //关闭
PWMPOL=0xff; //先高电平
PWMCAE=0x00; //左对齐
PWMCLK=0x0c; //选择0，1，4，5通道时钟源头A 2，3通道时钟源头SB,6,7为时钟B
PWMPRCLK=0x33; //时钟预分频0通道为8
//设置舵机
PWMCTL_CON01=1; //使得通道0，1成为16位pwm
PWMPER0 =0x75;
PWMPER1 =0x30; //舵机的频率是： 24M/8/30000=100Hz,T=10ms
PWMDTY01=4500; // 对应为4500/30000的占空比,待调整
//设置电机
PWMSCLB =50; // 24MHZ/8/2/50=300K
PWMPER2 =100; //电机的频率是： 24M/8/2/50/500=300Hz,T=5ms
PWMDTY2 =0; //用于测试电机
PWMPER3 =100; //电机的频率是： 24M/8/2/50/500=300Hz,T=5ms
PWMDTY3 =0; //用于测试电机
//小灯输出
PWMPER4 =79; //小灯的频率是： 24M/8/79=38kHz
PWMDTY4 =30;
PWMPER7 =79; //小灯的频率是： 24M/8/79=38kHz
PWMDTY7 =30;
DDRP =0xff; //控制输出
PWME =0x9f; //开放通道0，1 ,3,4,5,6,7
}
void PACBInit()
{
TCTL4 = 0X02; //下降沿捕捉脉冲
PBCTL = 0x40; //级联两个8位累加器(PAC0和PAC1)
ICPAR = 0X03; //使能累加器
PACN10 = 0X0000;
}
void MDCInit(void)
{
MCCTL = MCCTL&0Xfb; //模数计数器禁止运行
MCCTL = 0Xe3; //允许中断，模数计数方式
//返回时重新加载所用的常数，分频常数为16
MCCTL = MCCTL|0X04; //模数计数器使能
MCCNT = 1500; //(1/24M)*16*1500= 1ms
MCCTL = MCCTL|0X08 ; //把模数常数寄存器的值加载到模数计数器FLMC;
}
void set_parameter(void)//----------参数设置----------
{
blackline=500;
tan_n=0;
qidian=0,once=0;
speed=0;
counter_a=0,counter_b=0; //前后两排在黑线阀值上的灯数
one=5;two=4;three=6;x=1; //前后排灯在黑线上的灯编号
biga=0;bigb=0;bigc=0; bigd=0; //前排最大的三个电压值he后排的最大值
tan_o=0,tan_n=0; //前，后次偏角正弦
tan_n1=0;tan_n2=0;tan_n3=0;tan_n4=0;tan_n5=0;
away_o=0,away_n=0; //前后次车身中线偏移量
kj=0;kp=0;
zhuan=0;
}
//----------红外检测---------
void Infrared_detect(void)
{
while(！ATD0STAT1_CCF0);
qian[0] = ATD0DR0;
while(！ATD0STAT1_CCF1);
qian[1] = ATD0DR1;
while(！ATD0STAT1_CCF2);
qian[2] = ATD0DR2;
while(！ATD1STAT1_CCF4);
qian[3] = ATD1DR4;
while(！ATD0STAT1_CCF3);
qian[4] = ATD0DR3;
while(！ATD0STAT1_CCF4);
qian[5] = ATD0DR4;
while(！ATD0STAT1_CCF5);
qian[6] = ATD0DR5;
while(！ATD1STAT1_CCF5);
qian[7] = ATD1DR5;
while(！ATD0STAT1_CCF6);
qian[8] = ATD0DR6;
while(！ATD0STAT1_CCF7);
qian[9] = ATD0DR7;
while(！ATD1STAT1_CCF0);
qian[10] = ATD1DR0;
while(！ATD1STAT1_CCF1);
hou[0] = ATD1DR1;
while(！ATD1STAT1_CCF2);
hou[1] = ATD1DR2;
while(！ATD1STAT1_CCF3); //AD0所有和AD1前4个用作探测信号接受
hou[2] = ATD1DR3;
while(！ATD1STAT1_CCF6); //档位通道开始
dw_result[0] = ATD1DR6;
while(！ATD1STAT1_CCF7);
dw_result[1] = ATD1DR7;
//档位通道结束
}
void data_handle(void) //数据处理
{
int n;
counter_a=0,counter_b=0;
for(n=0;n<=10;n++)
{
if(qian[n]>blackline)
counter_a++;
}
for(n=0;n<=2;n++)
{
if(hou[n]>blackline)
counter_b++;
}
dangwei=0;
for(n=0;n<3;n++)
{
if(dw_result[n]>600) dangwei++; // 统计档位数目
}
switch( one ) //for testing
{
case 10: PORTB=0xf0;break;
case 9: PORTB=0xfe;break;
case 8: PORTB=0xfd;break;
case 7: PORTB=0xfb;break;
case 6: PORTB=0xf7;break;
case 5: PORTB=0xaa;break;
case 4: PORTB=0xef;break;
case 3: PORTB=0xdf;break;
case 2: PORTB=0xbf;break;
case 1: PORTB=0x7f;break;
case 0: PORTB=0x0f;break;
default:break;
}
{//判断起始点，参数待调整
if(((qian[1]>blackline&&qian[9]>blackline&&qian[2]>blackline
&&qian[10]>blackline)||(qian[1]>blackline&&qian[9]>blackline
&&qian[0]>blackline&&qian[8]>blackline))&&(qian[3]<blackline
||qian[4]<blackline||qian[6]<blackline||qian[7]<blackline)
&&(counter_a>4&&counter_a<8))
{
once++;tan_n=tan_o;away_n=away_o;
}
if(once>1&&counter_a<4)
{
if(go==2){qidian=1;once=0;}
if(go==3){qidian++;once=0;}
}
}
if(counter_a>5&&qian[4]>blackline&&qian[5]>blackline&&qian[6]>blackline&&
(qian[3]>blackline||qian[7]>blackline)) //过十字交叉线时或过起始线
{tan_n=tan_o; away_n=away_o;}
biga=0;
for(n=0;n<=10;n++) //寻找最大值
{if(biga<qian[n]){biga=qian[n];one=n;}}
bigb=0;
for(n=0;n<=10;n++) //寻找第二大值
{if((bigb<qian[n])&&n！=one){bigb=qian[n];two=n;}}
bigc=0;
for(n=0;n<=10;n++) //寻找第三大值
{if((bigc<qian[n])&&n！=one&&n！=two){bigc=qian[n];three=n;}}
bigd=0;
for(n=0;n<=2;n++)
{if(bigd<hou[n]){bigd=hou[n];x=n;}}
if(counter_a>5&&qian[4]>blackline&&qian[5]>blackline&&qian[6]>blackline)
one=5;
if((one-two)==1||(two-one)==1)
{
if(counter_a>0&&counter_a<3)
{
if( (biga-bigb)<=(bigb-bigc) ) tan_n=(pianxin_a[one]+pianxin_a[two])/2;
else tan_n=pianxin_a[one];
if(counter_b>0)
{
switch(x)
{case 1:
{
away_n=0; break;
}
case 0:
{
away_n=34; break;
}
case 2:
{
away_n=-34; break;
}
}
}
if(counter_b==0)
{
if(tan_n<=20&&tan_n>=-20)away_n=0;
if(tan_n>20)away_n=17;
if(tan_n<-20)away_n=-17;
}
}
if(counter_a==0)
{
if(tan_o<-100)
tan_n=-120;
if(tan_o>100)
tan_n=120;
if(tan_o<100&&tan_o>-100)
tan_n=tan_o;
away_n=away_o;
}
}
tan_n1=tan_n2;tan_n2=tan_n3;tan_n3=tan_n4;tan_n4=tan_n5;tan_n5=tan_n6;tan_n6=tan_n;
tan_n=(tan_n4+tan_n5+tan_n6+tan_n3+tan_n2+tan_n1)/6;
if(tan_o<50&&tan_n>-50){
if(tan_n-tan_o<-10||tan_n-tan_o>10) {
tan_n=tan_o;
if((tan_n5-tan_n4>5&&tan_n4-tan_n3>4&&tan_n3-tan_n2>3)||(tan_n5-tan_n4<-5&&tan_n4-
tan_n3<-4&&tan_n3-tan_n2<-3)
||(tan_n5-tan_n4>0&&tan_n4-tan_n3>0&&tan_n3-tan_n2>0&&tan_n2-tan_n1>0)
||(tan_n5-tan_n4<0&&tan_n4-tan_n3<0&&tan_n3-tan_n2<0&&tan_n2-tan_n1<0))
tan_n=tan_n6;
}
}
if(tan_o>50||tan_n<-50){
if(tan_n-tan_o<-15||tan_n-tan_o>15) {
tan_n=tan_o;
if((tan_n5-tan_n4>5&&tan_n4-tan_n3>4&&tan_n3-tan_n2>3)||(tan_n5-tan_n4<-5&&tan_n4-
tan_n3<-4&&tan_n3-tan_n2<-3)
||(tan_n5-tan_n4>0&&tan_n4-tan_n3>0&&tan_n3-tan_n2>0&&tan_n2-tan_n1>0)
||(tan_n5-tan_n4<0&&tan_n4-tan_n3<0&&tan_n3-tan_n2<0&&tan_n2-tan_n1<0))
tan_n=tan_n6;
}
}
tan_o=tan_n;away_o=away_n;
}
void direct_ctl(void)
{
if(tan_n>=-120&&tan_n<-90){kj=46;kp=12;}
if(tan_n>=-90&&tan_n<-60) {kj=47;kp=13;}
if(tan_n>=-60&&tan_n<-20) {kj=48;kp=14;}
if(tan_n>=-20&&tan_n<-8) {kj=46;kp=15;}
if(tan_n>=-8&&tan_n<0) {kj=45;kp=14;}
if(tan_n>=0&&tan_n<8) {kj=45;kp=14;}
if(tan_n>=8&&tan_n<20) {kj=46;kp=15;}
if(tan_n>=20&&tan_n<60) {kj=48;kp=14;}
if(tan_n>=60&&tan_n<90) {kj=47;kp=13;}
if(tan_n>=90&&tan_n<=120) {kj=47;kp=12;}
zhuan=(tan_n*kj+away_n*kp)/10;
}
void steer_ctl(void) //舵机控制
{
direct_ctl();
PWMDTY01=4620+zhuan;
}
//刹车函数参数待调整 j<?
void slow1(void)
{
int i;int j;
for(i=1;i<6;i++)
for(j=1;j<6;j++)
{
PWMDTY2=0;
PWMDTY3=10;}
PWMDTY3=0;
}
void slow2(void)
{
int i;int j;
for(i=1;i<8;i++)
for(j=1;j<7;j++)
{
PWMDTY2=0;
PWMDTY3=12;}
PWMDTY3=0;
}
void slow3(void)
{
int i;int j;
for(i=1;i<8;i++)
for(j=1;j<8;j++)
{ PWMDTY2=0;
PWMDTY3=14;}
PWMDTY3=0;
}
void slow4(void)
{
int i;int j;
for(i=1;i<10;i++)
for(j=1;j<15;j++)
{ PWMDTY2=0;
PWMDTY3=16;}
PWMDTY3=0;
jia_1++;}
void slow5(void)
{
int i;int j;
for(i=1;i<12;i++)
for(j=1;j<15;j++)
{ PWMDTY2=0;
PWMDTY3=18;}
PWMDTY3=0;
jia_2++;}
void slow6(void)
{
int i;int j;
for(i=1;i<15;i++)
for(j=1;j<13;j++)
{ PWMDTY2=0;
PWMDTY3=20;}
PWMDTY3=0;
jia_3++;}
void speed_ctl(void)
{
sudu=167*Get_pulse/10; //单位cm/s
if(zhuan<50&&zhuan>-50)speed=42;
if((zhuan<80&&zhuan>=50)||(zhuan<=-50&&zhuan>-80))speed=41;
if((zhuan<110&&zhuan>=80)||(zhuan<=-80&&zhuan>-110))speed=40;
if((zhuan<140&&zhuan>=110)||(zhuan<=-110&&zhuan>-140))speed=39;
if((zhuan<170&&zhuan>=140)||(zhuan<=-140&&zhuan>-170))speed=38;
if((zhuan<200&&zhuan>=170)||(zhuan<=-170&&zhuan>-200))speed=37;
if((zhuan<230&&zhuan>=200)||(zhuan<=-200&&zhuan>-230))speed=38;
if((zhuan<270&&zhuan>=230)||(zhuan<=-230&&zhuan>-270))speed=39;
if((zhuan<310&&zhuan>=270)||(zhuan<=-270&&zhuan>-310))speed=40;
if((zhuan<350&&zhuan>=310)||(zhuan<=-310&&zhuan>-350))speed=40;
if((zhuan<380&&zhuan>=350)||(zhuan<=-350&&zhuan>-380))speed=40;
if((zhuan<400&&zhuan>=380)||(zhuan<=-380&&zhuan>-400))speed=40;
if((zhuan<440&&zhuan>=400)||(zhuan<=-400&&zhuan>-440))speed=39;
if((zhuan<490&&zhuan>=440)||(zhuan<=-440&&zhuan>-490))speed=38;
if((zhuan<550&&zhuan>=490)||(zhuan<=-490&&zhuan>-550))speed=37;
if((zhuan<600&&zhuan>=550)||(zhuan<=-550&&zhuan>-600))speed=36;
if(zhuan>600||zhuan<-600)speed=35;
}
void motor_ctl(void)
{ int i; int t;
speed_ctl();
if(go>0)
{
if(qidian<2){
switch(dangwei){
case 0: {
if(zhuan>530||zhuan<-530)
{
if(sudu>230)slow3();
}
t=speed+25-sudu/10;
if(t>30&&t<70)
PWMDTY2=speed+25-sudu/10;break;
}
case 1: {
if(zhuan>300||zhuan<-300)
{
if(sudu>300)slow2();
if(sudu>330)slow3();
}
if((zhuan<=500&&zhuan>300)||(zhuan<-300&&zhuan>=-500))
{
if(sudu>280)slow4();
}
if(zhuan>500||zhuan<-500)
{
if(sudu>220)slow5();
}
if(jia_1+jia_2+jia_3>1)
{ for(i=0;i<500;i++) {
PWMDTY2= 75;
}
jia_1=0;jia_2=0;jia_3=0;
}
t=speed+35-sudu/10;
if(t>20&&t<70)
PWMDTY2=speed+38-sudu/12;break;
}
case 2: {
if(zhuan>280||zhuan<-280)
{
if(sudu>250)slow2();
if(sudu>290)slow3();
}
if((zhuan<=360&&zhuan>200)||(zhuan<-200&&zhuan>=-360))
{
if(sudu>240)slow4();
}
if((zhuan<=530&&zhuan>360)||(zhuan<-360&&zhuan>=-530))
{
if(sudu>220)slow5();
}
if(zhuan>520||zhuan<-520)
{
if(sudu>220)slow6();
}
if(jia_1+jia_2+jia_3>2)
{ for(i=0;i<600;i++) {
PWMDTY2= 80;
}
jia_1=0;jia_2=0;jia_3=0;
}
t=speed+70-sudu/9;
if(t>20&&t<90)
PWMDTY2=speed+70-sudu/9;break;
}
default: PWMDTY2=50;break;
}
}
else
{
PWMDTY2=0;
}
}
这个是干什么的?
谢谢小编分享！