CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Diff: main.cpp
- Revision:
- 5:c8e4d5cdc353
- Parent:
- 4:7c488c059498
- Child:
- 6:2c9cb40ada59
diff -r 7c488c059498 -r c8e4d5cdc353 main.cpp --- a/main.cpp Fri Mar 13 02:10:37 2015 +0000 +++ b/main.cpp Fri Mar 13 03:17:44 2015 +0000 @@ -81,7 +81,7 @@ Degree = Degree*(-1.0); CompassDif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE; - if(abs(CompassDif)<CompassVias){ + if(/*abs(CompassDif)<CompassVias*/1){ CompassVias=15.0; @@ -415,20 +415,6 @@ 6 */ - if( - ( - ( - ( - (Compass / 10) + 540 - CompassDef - ) % 360 - ) - REFERENCE - ) >10.0 - ){ - - move(0,0,0,0); - return; - } - for(int i=0; i<20; i++) State[i]=0; @@ -442,13 +428,15 @@ if(PingData[2] <=40+10) State[6]=1; - if(PingData[2] >=10) State[7]=1; + if(PingData[2] >=30) State[7]=1; - if(PingData[2] <=5) State[8]=1; + if(PingData[2] <=10) State[8]=1; - if(PingData[2] >=25-5) State[9]=1; + if((PingData[2] >=15)&&(PingData[2] <=25)) State[9]=1; - if(PingData[2] <=5+5) State[10]=1; + if(PingData[2] <15) State[10]=1; + + if(PingData[2] >25) State[11]=1; @@ -472,50 +460,29 @@ }*/ if(State[4]){ - - if(State[3]){ - - if((State[7])==(State[8])){ - - move(0,0,CompassPID,LastDegree); - return ; - } - if(State[7]){ - - move(vrl[6],vrl[6],CompassPID,IrServo[6]); - return ; - } - if(State[8]){ - - move(vrl[0],vrl[0],CompassPID,IrServo[0]); - return ; - } + + if(State[9]){ + moving(12); + return ; + } + if(State[10]){ + moving(0); + return ; + } + if(State[11]){ + moving(6); + return ; } - - if((State[0])==(State[1])){ - - move(0,0,CompassPID,LastDegree); - return ; - } - if(State[0]){ - - move(vrl[6],vrl[6],CompassPID,IrServo[6]); - return ; - } - if(State[1]){ - - move(vrl[0],vrl[0],CompassPID,IrServo[0]); - return ; - } + }else{ if(State[2]){ - move(vrl[9],vrl[9],CompassPID,IrServo[9]); + moving(9); return ; } else{ - move(vrl[3],vrl[3],CompassPID,IrServo[3]); + moving(3); return ; } } @@ -728,7 +695,7 @@ //int addDegree = 0;//追加角 /**/ - //int Gap=2; + int Gap=2; bool LineState[10]={0}; @@ -752,19 +719,19 @@ while(1){ - if( + /*if( ( ( ( (Compass / 10) + 540 - CompassDef ) % 360 ) - REFERENCE - ) >15.0 + ) >=15 ){ move(0,0,0,0); continue; - } + }*/ LineData = (~Line+0x00) & 0x0F; @@ -773,7 +740,7 @@ if(LineData!=0){ if(PingData[0]>PingData[1]) LineState[0]=1; - if(PingData[2]<=20) LineState[1]=1; + if(PingData[2]<=30) LineState[1]=1; switch(LinkBit(0,0,0,0,0,0,LineState[1],LineState[0])&0x03){ @@ -801,9 +768,9 @@ continue; }else{ - if(PingData[0]<=5) LineState[2]=1; - if(PingData[1]<=5) LineState[3]=1; - if(PingData[2]<=5) LineState[4]=1; + if(PingData[0]<=10) LineState[2]=1; + if(PingData[1]<=10) LineState[3]=1; + if(PingData[2]<=10) LineState[4]=1; switch(LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07){ @@ -849,47 +816,40 @@ wait(0.2); continue; } - else{ - - moving(12); - continue; + + + switch(/*IrNum*/12){ + case 0: + case 1: + case 2: + case 3: + moving(IrNum); + break; + case 4: + case 5: + moving(IrNum+Gap); + break; + case 6: + if(PingData[0]>PingData[1]) moving(IrNum+Gap); + else moving(IrNum-Gap); + + break; + case 7: + case 8: + moving(IrNum-Gap); + break; + case 9: + case 10: + case 11: + moving(IrNum); + break; + case 12: + GoHome(); + break; + default: + break; } + } - /* - Gap=2; - - switch(IrNum){ - case 0: - case 1: - case 2: - case 3: - moving(IrNum); - break; - case 4: - case 5: - moving(IrNum+Gap); - break; - case 6: - if(PingData[0]>PingData[1]) moving(IrNum+Gap); - else moving(IrNum-Gap); - - break; - case 7: - case 8: - moving(IrNum-Gap); - break; - case 9: - case 10: - case 11: - moving(IrNum); - break; - case 12: - GoHome(); - break; - - } - */ - - }