Led_left=1; }
if(z==2) {
FB=90; SH=45; SH1=45; V1=85; V2=85; V3=80; V4=150; V5=90; V6=80;
Led_right=0; Led_left=1; delay(500); Led_right=1; }
if(z==3) {
FB=100; SH=45; SH1=45; V1=95; V2=95; V3=90; V4=160; V5=100; V6=100;
Led_right=0; Led_left=0; delay(500); Led_right=1; Led_left=1; }
if(z==4) {
z=0; FB=100; SH=45; SH1=45;
V1=95; V2=95; V3=90; V4=170; V5=100; V6=100;
Led_right=1; Led_left=0; delay(500); Led_left=1; } } } }
//////////////////////////////////// ///////////速度控制///////////////// //////////////////////////////////// void Speed_con(void) {
while(!ATD0STAT0_SCF); //等待当前队列转换完成*/ AD_1=ATD0DR0L; AD_2=ATD0DR1L; AD_3=ATD0DR2L; AD_4=ATD0DR3L; AD_5=ATD0DR4L; AD_6=ATD0DR5L;
//////////////////////////////// /////////斜坡检测////////////// ///////////////////////////////
//if(AD_1>90 && AD_4>90 || AD_1<50 && AD_4<50) //{
// PWMDTY23=1565; //Sudu(num_last2); //} //else // {
if(AD_1>3 || AD_4>3) {
x1=AD_2-82; //中间左 x2=AD_3-82; //中间右 if(AD_1
if(AD_4
y=Steer_PID(x1,x2); //舵机PID if(y>650) y=650;
if(y<-650) y=-650;
numm=1570-y; PWMDTY23=numm;
if(AD_1>40 || AD_4>40) //弯道 {
if(flag==1 && FeedBack>FB) {
if(AD_1>AD_4) {
Led_left=0; PWMDTY23=numm; shacheV(SH); Sudu(V1); }
else {
Led_right=0; PWMDTY23=numm; shacheV(SH1); Sudu(V2); } }
else {
Sudu(V3); }
//////////////////////////////// /////////保存numm值///////////// //////////////////////////////// if(5
numm_last0=numm; }
}
else //直道 {
if(AD_2>50 && AD_3>50 && AD_5>40 && AD_6>40) {
//////////斜坡检测//////////////
if(AD_1>95 && AD_4>95 || AD_1<30 && AD_4<30) {
PWMDTY23=1570; Sudu(num_last2); } else {
PWMDTY23=1570; flag=1; Sudu(V4);
num_last0=FeedBack; } }
else {
Sudu(V5); } }
numm_last1=numm_last0; numm_last2=numm_last1; }
else {
PWMDTY23=numm_last2; Sudu(V6); }
num_last1=num_last0; num_last2=num_last1; if(num_last2==0) {
num_last2=100; }
}
//----- 时钟初始化程序 --------// void SetBusCLK_64M(void) {
CLKSEL=0X00; //disengage PLL to system PLLCTL_PLLON=1; //turn on PLL
SYNR =0xc0 | 0x07; REFDV=0x80 | 0x01;
POSTDIV=0x00; //pllclock=2*osc*(1+SYNR)/(1+REFDV)=128MHz; _asm(nop); //BUS CLOCK=64M _asm(nop);
while(!(CRGFLG_LOCK==1)); //when pll is steady ,then use it; CLKSEL_PLLSEL =1; //engage PLL to system; }
//-------------PWM初始化程序----------// void PWM_Init() {
PWME=0;
PWMPRCLK=0X53; //时钟预分频设置 A=4MHz B=2MHz PWMCLK=0x03; //时钟源选择SA,SA,B
PWMSCLA=0X02; //分频寄存器，时钟设置SA=A/2^PWMSCLA=1MHz PWMPOL=0x0f; //极性设置，开始输出高电平
PWMCAE=0x0f; //对齐方式选择1为中心对齐（通道关闭后才能对其设置)
PWMCTL_CON23=1; //通道 2,3 级联成一个16位的PWM通道 PWMDTY0=0x32; //占空比设置 PWMDTY1=0x32; //占空比设置 PWMDTY23=0; //占空比设置
PWMPER0=0x64; //周期寄存器设置Frequency=SA/10=10KHz PWMPER1=0x64; //周期寄存器设置Frequency=SA/10=10KHz PWMPER23=0x2710; //周期寄存器设置Frequency=B/10000=100Hz PWMCNT0=0X00; //通道0计数寄存器 PWMCNT1=0X00; //通道1计数寄存器 PWME=0x0f; }