CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Diff: Loop.txt
- Revision:
- 6:2c9cb40ada59
- Parent:
- 4:7c488c059498
diff -r c8e4d5cdc353 -r 2c9cb40ada59 Loop.txt --- a/Loop.txt Fri Mar 13 03:17:44 2015 +0000 +++ b/Loop.txt Sat Mar 14 01:06:13 2015 +0000 @@ -197,4 +197,181 @@ while(0){ moving(6); - } \ No newline at end of file + } + + + + + /*3/13(Fri)*/ + + + if(LineData!=0){ + + if(PingData[0]>PingData[1]) LineState[0]=1; + if(PingData[2]<=30) LineState[1]=1; + + switch(LinkBit(0,0,0,0,0,0,LineState[1],LineState[0])&0x03){ + + case 0: + moving(3); + break; + case 1: + moving(9); + break; + case 2: + moving(1); + break; + case 3: + moving(11); + break; + default : + break; + + } + /*if(LastPwm[0]==0){ + + } + move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg);*/ + wait(0.5); + continue; + + }else{ + /*if(PingData[0]<=15) LineState[2]=1; + if(PingData[1]<=15) LineState[3]=1; + if(PingData[2]<=15) LineState[4]=1; + + switch(LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07){ + + case 0: + break; + case 1: + moving(3); + break; + case 2: + moving(9); + break; + case 3: + moving(0); + break; + case 4: + moving(0); + break; + case 5: + moving(1); + break; + case 6: + moving(11); + break; + case 7: + moving(0); + break; + default : + break; + + } + + if((LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07)!=0){ + wait(0.2); + continue; + }*/ + } + + + + + 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; + default: + break; + } + + + /*3/14(Sat)*/ + + + + if((IrNum==12)&& + (!( + (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) + <= 80 + )) && + ( + PingData[2]<=50 + + )&& + ( + (abs(CompassDif)<5) + ) + + + ){ + + if(PingData[0]>PingData[1]){ + moving(10); + wait(0.5); + continue; + } + else{ + moving(2); + wait(0.5); + continue; + } + + } + + if((PingData[2]>100)&& + ( + (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) + <= 80 + ) + ){ + + move(vrl[6]-10,vrl[6]-10,0,IrServo[6]); + continue; + } + if((PingData[2]>60)&& + ( + (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) + <= 60 + )&&(IrNum==12) + ){ + + move(vrl[6]-5,vrl[6]-5,0,IrServo[6]); + continue; + } + if((PingData[2]>30)&& + ( + (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) + <= 60 + )&&(IrNum==12) + ){ + + move(vrl[6],vrl[6],0,IrServo[6]); + continue; + } \ No newline at end of file