上海电力学院电自二队技术报告
pvw1=67;
wandao2=1200; pvw2=64;
}else if(dwei==3) {
zhidao=1700; pvz=68;
wandao1=1400; pvw1=64;
wandao2=1000; pvw2=62;
}else if(dwei==4) {
zhidao=1500; pvz=68;
wandao1=1200; pvw1=60;
wandao2=1200; pvw2=60;
} else if(dwei==5) {
zhidao=1200; pvz=63;
wandao1=1200; pvw1=60;
wandao2=1200; pvw2=60;
}else if(dwei==6) {
zhidao=1200; pvz=55;
wandao1=1200; pvw1=55;
wandao2=1200; pvw2=55;
} else if(dwei==7) {
zhidao=1000; pvz=50;
wandao1=1000;
31
上海电力学院电自二队技术报告
pvw1=50;
wandao2=1000; pvw2=50; }
PORTA_PA0=~PORTA_PA0; ADsave();
weizhi1=(ad_m-wb)/xielv1; adjian1=ad_z-ad_y;
if(weizhi1>0&&weizhi1<20){ if(adjian1>0){ fuhao=1; }else{
fuhao=-1; } }
if(adjian1<0)
adjian1=-adjian1; weizhi=weizhi01;
weizhi01=2*(float)adjian1/xielv2*fuhao; dz=ad_fz-fz; dy=ad_fy-fy; if(dz<0) dz=0; if(dy<0) dy=0;
k1=dz/yjj; k2=dy/yjj; k=k1-k2; if(!shizi)
weizhi01=(k+weizhi01)/2; weizhi0=(int)weizhi01; if(weizhi01>19) { weizhi01=19; weizhi0=19;
}else if(weizhi01<-19){ weizhi01=-19; weizhi0=-19; }
if((k1>2)&&(k2>2)){ shizi=1;
32
上海电力学院电自二队技术报告
}else shizi=0;
if(weizhi1>0&&weizhi1<50){
change1=change0[weizhi0+20];
chg=(int)((change0[weizhi0+21]-change0[weizhi0+20])*(weizhi01-(float)weizhi0));
dch=(int)((weizhi01-weizhi)*200); old_change1=change1; change=change1+dch+chg; }else{
old_change1+=5*fuhao; if(old_change1>640) old_change1=640;
else if(old_change1<-640) old_change1=-640; change=old_change1; }
PWMDTY45=4795+change; //PORTE_PE7=shizi; if(shizi) type=1; else{
if(k<7&&k>-7) { if(type!=1) {
if(count2>500) { count2=0; count3=0; count4=0;
type=1; } else
count2++; }
}else if((k>=7&&k<=20)||(k<=-7&&k>=-20)){ if(type!=2) { if(count3>10){ count2=0; count3=0; count4=0;
33
上海电力学院电自二队技术报告
type=2; }else
count3++; }
}else {
if(type!=3) { if(count4>30){ count2=0; count3=0; count4=0; type=3; }else
count4++; } } }
if(flag==1) { flag=0;
if(type==1) { count5=0; if(speed>pvz)
if(speed>pvz+8) xlb=zhidao-1000; else
xlb=zhidao-500; else if(pvz-speed>6) {
xlb=zhidao+(pvz-speed)*20; if(xlb>2400) xlb=2400; } else
xlb=zhidao; }else if(type==2){
if((old_type==1)&&(count5==0)) { count5=25;
}else if(count5==0){ if(speed>pvw1) xlb=wandao1-500; else
34
上海电力学院电自二队技术报告
xlb=wandao1; } }else {
if(speed>pvw2)
xlb=wandao2-500; else
xlb=wandao2; }
if(count5!=0)
if(speed<54) {
xlb=wandao1-200; count5--;
if(change<200&&change>-200) { xlb=zhidao; count5=0; } }else {
count5--;
//PORTE_PE7=1; if(speed>68)
xlb=-300+(speed-68)*25; else if(speed>64) xlb=-200;
else if(speed>62) xlb=-100;
else if(speed>60) xlb=0;
else if(speed>57) xlb=50;
else if(speed>54) xlb=100; }
old_type=type; }
if(type==1) PORTE_PE7=1; else
PORTE_PE7=0;
35
上海电力学院电自二队技术报告
//if(xlb<=0&&speed!=0) //xlb=0;
if(ad_z<(xielv2+20)&&ad_y<(xielv2+20)&&ad_m<(xielv2+20)) if(dwei==0) xlb=0; else
xlb=10;
if(qishixian&&type==1) { count++;
if(count==3000) { count=0;
qishixian=0; qishicishu++; } }
if(qishicishu>=2&&PORTA_PA5) { if(speed>3) { xlb=speed*20; xlb=-xlb; } else
xlb=0; } if(xlb<0) {
if(dwei!=0) { Backward(); PWMDTY23=-xlb; }else{ xlb=0; } }else {
Forward(); PWMDTY01=xlb; }
} }
36