if(PTGD_PTGD4==0) {
delay1(1);
if (PTGD_PTGD4==0) {
delay2(10) ;
EnableInterrupts; break ; } }
__RESET_WATCHDOG(); } }
/********************起始线识别*******************************/ void StartLine() {
if((ad_data[7]<=30||ad_data[8]<=30)&&ad_data[10]>=70&&(ad_data[5]>=100||ad_data[6]>=100)&&(ad_data[3]<=30||ad_data[4]<=30)&&ad_data[1]>=70) {
circle++ ;
PTGD_PTGD6=1; delay(8000);
PTGD_PTGD6=0; } }
/********************************************************************/ void main(void) {
IOInit(); ICGInit(); ADCInit(); servoInit(); PIDInit();
// SOPT_COPE=0; TPM3SC = 0x4F; /*总线时钟128分频*/ TPM3MOD = 2500; DisableInterrupts; // EnableInterrupts; Start_Key(); for(;;) {
get_data();
StartLine();
n=zw[10]*1024+zw[9]*512+zw[8]*256+zw[7]*128+zw[6]*64+zw[5]*32+zw[4]*16+zw[3]*8+z
36
w[2]*4+zw[1]*2+zw[0];
m=zw[6]*64+zw[5]*32+zw[4]*16+zw[3]*8+zw[2]*4+zw[1]*2+zw[0]; if((m==124)||(m==120)||(m==112)) //直道入弯 {
if((speed>4)&&(r==1)) {
// v=TPM2C1V-(Turn-14)*speed; Stop1; }
if((speed>4)&&(r==2)) {
// v=TPM2C1V+(8-Turn)*speed; Stop1; }
u=100*speed*(abs(Turn-12)); r=0;
SteerContral(); SpeedContral(); }
//大S道 else if(m==0) {
if(speed>12) { Stop1;
TPM2C1V=TPM2C1V+cangshu[Turn]*speed; } else {
PTCD_PTCD6=0; PTFD_PTFD7=1;
u=90000/(speed*(abs(Turn-12)));//+speedPIDCalc(&sPID) }
SteerContral(); SpeedContral(); } else {
if(Turn==10||Turn==11||Turn==12) {
if(speed<13) {
u=2500;
PTCD_PTCD6=0;
37
PTFD_PTFD7=1; }
else if(speed>20) {
Stop1;
u=70*speed; } else {
u=14000/speed ; //u=900 PTCD_PTCD6=0; PTFD_PTFD7=1; }
v=191;
SteerContral(); SpeedContral(); }
if(((Turn > 6) && (Turn < 10)) && ((speed > 3)&&(speed<13))) {
PTCD_PTCD6=0; PTFD_PTFD7=1;
u=50000/(speed*(12-Turn)) ; //u=1250 SpeedContral(); }
if(((Turn > 12) && (Turn < 16)) && ((speed > 3)&&(speed<13))) {
PTCD_PTCD6=0; PTFD_PTFD7=1;
u=50000/(speed*(Turn-12)); //u=1250 SpeedContral(); }
if(((Turn > 6) && (Turn < 10)) && (speed > 12)) {
Stop1;
u=25*speed*(12-Turn); SpeedContral(); }
if(((Turn > 12) && (Turn < 16)) && (speed > 12)) {
Stop1;
u=25*speed*(Turn-12); SpeedContral(); }
if(((Turn> 3) && (Turn < 7)) && (speed <10))
38
{
PTCD_PTCD6=0; PTFD_PTFD7=1;
u=30000/(speed*(12-Turn)); SpeedContral(); }
if(((Turn> 15) && (Turn< 19)) && (speed <10)) { PTCD_PTCD6=0; PTFD_PTFD7=1;
u=30000/(speed*(Turn-12)); SpeedContral(); }
if(((Turn > 3) && (Turn < 7)) && (speed > 9)) { Stop1;
u=15*speed*(12-Turn); SpeedContral(); }
if(((Turn> 15) && (Turn < 19)) && (speed > 9)) {
Stop1;
u=15*speed*(Turn-12); SpeedContral(); }
if(((Turn> 18) || (Turn< 4)) && (speed < 8)) { PTCD_PTCD6=0; PTFD_PTFD7=1; if(Turn > 18) {
u=10000/(speed*(Turn-12)); }
if(Turn <4) {
u=10000/(speed*(12-Turn)); }
SpeedContral(); }
if(((Turn > 18) || (Turn< 4)) && (speed >10)) {
Stop1;
u=20*(abs(Turn-12))*speed; SpeedContral(); } else {
39
PTCD_PTCD6=0; PTFD_PTFD7=1; SpeedContral(); }
SteerContral(); }
__RESET_WATCHDOG(); } }
40