![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Diff: main.cpp
- Revision:
- 6:2c9cb40ada59
- Parent:
- 5:c8e4d5cdc353
- Child:
- 7:81f57b67dff8
diff -r c8e4d5cdc353 -r 2c9cb40ada59 main.cpp --- a/main.cpp Fri Mar 13 03:17:44 2015 +0000 +++ b/main.cpp Sat Mar 14 01:06:13 2015 +0000 @@ -70,21 +70,55 @@ void Interval(void){ WaitFlag=0; } +void Interval3(void){ + CompassFlag=0; + //HomeFlag=0; +} +void Interval2(void){ + + CompassFlag=1; + //CompassFlag=0; + DTimer.attach(Interval3, 1.25); + +} + void move(int vr,int vl, double vs ,int Degree){ double pwm[4] = {0}; uint8_t i = 0; - double CompassVias=15.0; - double CompassDif; + + int dec = 0; + static double CompassVias=15+dec; + + // bool BackFlag; + + //BackFlag=0; + //if((vr==vrl[6])&&(Degree==IrServo[6])&&(PingData[2]>=100))BackFlag=1; + + if(IrNum==12){ + if(vr<0){ + vr=vr-(-0); + vl=vr-(-0); + } + else{ + vr=vr-0; + vl=vr-0; + } + } + + //CompassFlag=0; vr=vr*( 1.0); vl=vl*(-1.0); Degree = Degree*(-1.0); CompassDif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE; - if(/*abs(CompassDif)<CompassVias*/1){ + if( + (abs(CompassDif)<CompassVias )|| + CompassFlag/*((BackFlag==1)&&(abs(CompassDif)<45) )*/ + ){ - - CompassVias=15.0; + CompassVias=15+dec; + if(IrNum==12) CompassVias=15+dec; pwm[0] = LastPwm[0] = vr; pwm[1] = LastPwm[1] =0; @@ -97,17 +131,17 @@ } else{ - CompassVias=10.0; + CompassVias=10+dec; if(CompassDif<0){ //vs = vs - (20-CompassVias); vs = -(15+(abs(CompassDif))*0.1); - if( abs(CompassDif) >= 100) vs = -(20+(100)*0.1); + if( abs(CompassDif) >= 100) vs = -(15+(100)*0.1); } else{ //vs = vs + (20-CompassVias); vs = (15+(abs(CompassDif))*0.1); - if( abs(CompassDif) >= 100) vs = (20+(100)*0.1); + if( abs(CompassDif) >= 100) vs = (15+(100)*0.1); } pwm[0] = vs; @@ -116,8 +150,9 @@ pwm[3] = vs*(-1)*(-1); Degree = 0.0; + CTimer.attach(Interval2, 0.2); } - if(Loop==1){ + if(Loop|0){ pwm[0] = 0; pwm[1] = 0; pwm[2] = 0; @@ -142,6 +177,7 @@ if(WaitFlag==1) Degree = LastDegree; + SetDegree = LastDegree = Degree; S555.position(SetDegree*11.0/9.0); @@ -432,14 +468,13 @@ if(PingData[2] <=10) State[8]=1; - if((PingData[2] >=15)&&(PingData[2] <=25)) State[9]=1; + if((PingData[2] >=10)&&(PingData[2] <=30)) State[9]=1; - if(PingData[2] <15) State[10]=1; + if(PingData[2] <10) State[10]=1; if(PingData[2] >25) State[11]=1; - if(PingData[0]>PingData[1]) State[2]=1; if( @@ -449,7 +484,7 @@ if( (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) - <= 10 + <= 25 ) State[4]=1; @@ -459,30 +494,41 @@ return ; }*/ + + + if(State[4]){ - if(State[9]){ + + if(State[10]){ moving(12); return ; } - if(State[10]){ - moving(0); - return ; - } if(State[11]){ moving(6); return ; } - - + if(State[9]){ + moving(12); + HomeFlag=1; + return ; + } + else{ + moving(12); + HomeFlag=1; + return ; + } }else{ + if(State[2]){ moving(9); + //wait(0.5); return ; } else{ moving(3); + //wait(0.5); return ; } } @@ -567,7 +613,7 @@ pid.setMode(AUTO_MODE); //pid sed def pid.setSetPoint(REFERENCE); //pid sed def - for(int i=0; i<15; i++){ + for(int i=0; i<5; i++){ CompassDef = (Compass / 10); wait_ms(10); } @@ -719,6 +765,11 @@ while(1){ + if(IrNum!=12){ + HomeFlag=0; + } + //if(HomeFlag) + /*if( ( ( @@ -733,6 +784,8 @@ continue; }*/ + + LineData = (~Line+0x00) & 0x0F; for(int i=0; i<10; i++) LineState[i]=0; @@ -764,85 +817,73 @@ } move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg);*/ - wait(0.5); + //wait(0.5); continue; - }else{ - if(PingData[0]<=10) LineState[2]=1; - if(PingData[1]<=10) LineState[3]=1; - if(PingData[2]<=10) LineState[4]=1; + } + if( + (!( + (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) + <= 30 + )) + ){ - 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(6); - break; - case 4: - moving(0); - break; - case 5: - moving(1); - break; - case 6: - moving(11); - break; - case 7: - moving(0); - break; - default : - break; - + if(PingData[0]>PingData[1]){ + moving(9); + //wait(0.5); + continue; + } + else{ + moving(3); + //wait(0.5); + continue; } - if((LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07)!=0){ - wait(0.2); + } + if( + (( + (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) + <= 30 + )) && + ( + PingData[2]>15 + + ) + ){ + + moving(6); + //wait(0.5); continue; - } + } - if(PingData[2]>=90){ + switch(IrNum){ - moving(6); - wait(0.2); - continue; - } - - - switch(/*IrNum*/12){ - case 0: case 1: case 2: case 3: - moving(IrNum); - break; case 4: case 5: - moving(IrNum+Gap); + moving(3); 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); + moving(9); break; + + + case 6: + if(PingData[0]>PingData[1]) moving(7); + else moving(5); + break; + + case 0: case 12: GoHome(); break; @@ -851,5 +892,11 @@ } + + + + + + } -} +} \ No newline at end of file