2012飞思卡尔 哈尔滨工业大学智能车程序

#include "includes.h"
/*位置PID调节*/
#define Ki_p 0
#define Kp_p 9/20
#define Kd_p 20/18
void Va_0(char);//加速
void Va_1(char);//减速
void Va_2(char);//减速
void Va_3(char);//减速
void fun0(void);
void fun1 (void);
void fun2(void);
void fun3(void);
void fun4(void);
void fun5(void);
void fun6(void);
void delay2(int x);
#define cycle 2 //圈数
void startpointIdentify()//起始识别
/*起始点识别*/
{char RaceS1 = RWS[0]+RWS[9]+RWS[1]+RWS[8]+RWS[2]+RWS[7];
char RaceS2 = RWS[0]+RWS[1]+RWS[2]+RWS[3]+RWS[4]+RWS[5]+RWS[6]+RWS[7]+RWS[8]+RWS[9];
if((RaceS1==6)&&(!WB))// RaceS1==6保证是正向通过，（!WB）
{
WB=1; //出发点判断打开
Pulse=0; //开始记判断距离
}
if((RaceS2==10)&&(WB==1)&&(!WBOff)) //若RaceS2值为10表示通过的事交叉线
{
WBOff=1; //WBOff=1表示通过的是十字交叉线；
}
if((Pulse>=5000)&&(WB==1))//判断距离是5000个码盘脉冲，计数结束后进入最终判断，同时初始化数据以便下一次判断；
{if(WBOff==0)
++Start; //Start表示经过出发点的次数；
else
Start=Start;
WBOff=0;
WB=0;
Pulse=0;
// PORTA=Start;
}if(Start>=cycle+1)
{
WB1=1;
if(Pulse>=5000)
{
WB1=0;
Pulse=0;
//DstV=0;
stop_flag = 0;
//delay2(10000);
}
}
}
int e,es=0;
int ee[10];
int cach[201];
int DstV = 0;
char flag_0;
char flag_y;
char stop_flag = 1;
char a_flag;
void SystemInit()
{
flag_0 = -1;
CRGInit();
ECTInit();//使能脉冲计数器
ATDInit();
PWMInit();
PWME=0Xaa; //使能PWM输出
DDRA = 0x00;
EnableInterrupts;
DDRB = 0xff;
//PORTB = 0x55;
ADvalueInit();// 初始化
}
char RWS[10]; //为全局布尔数组
char WBOff=0,WB=0,WB1=0; //为全局布尔数int Pulse=0; //为型全局变量
char Start=0; //经过起始点次数，为全局变量void startpointIdentify();
void main(void)
{
SystemInit();
DstV =108;
flag_0 = 0;
for(;;)
{
if(flag_0==2)
{
biliY();
chushihua();//计算ie
cache_replace();
ie=-ie; //反向加到舵机上
cach[0]=(int)(ie*100); //处理完所有数据，
ee[0] = cach[0]*Kp_p;
e = cach[0]*Kp_p+(cach[0]-cach[7])*Kd_p;// /70*CurrentSpeed;
if(e>600)
e=600;
if(e<-600)
e=-600;
ee[3] = cach[0] -cach[2];
if(stop_flag == 1)
{
aa = PORTA;
aa = aa&0xf0;
switch(aa)
{
case 0x00:fun0();break;//冲弯道速度比较快,且第二圈速度会减速case 0x20:fun1();break;//冲弯道速度比较快
case 0x30:fun2();break;//匀速
case 0x50:fun3();break;//冲直道
case 0x80:fun4();break;//快速直道，
case 0x90:fun5();break;//稳定跑
default :fun3(); break;//
}
}
else
{
DstV = 2;
PWMDTY45 = 4400+e;
if(SysTimer>200)
{
DstV = 0;
for(;;)
{
PWMDTY45 = 4400;
}
}
}
startpointIdentify(); //起点识别，到指定圈数后停止flag_0 = 0;
ATDInit();
}
}
}
void Va_0(char x) //加速0
{
DstV=DstV+3;
if(DstV>=x)
DstV=x;
}
void Va_1(char x)//加速1
{
DstV=DstV+5 ;
if(DstV>=x)
DstV=x;
}
void Va_2(char x)//减速2
{
DstV=DstV-5;
if(DstV<=x)
DstV=x;
}
void Va_3(char x)//减速3
{
DstV=DstV-1;
if(DstV<=x)
DstV=x;
}。