CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Diff: main.cpp
- Revision:
- 4:7c488c059498
- Parent:
- 3:2f74791564c9
- Child:
- 5:c8e4d5cdc353
diff -r 2f74791564c9 -r 7c488c059498 main.cpp --- a/main.cpp Wed Mar 11 07:47:19 2015 +0000 +++ b/main.cpp Fri Mar 13 02:10:37 2015 +0000 @@ -64,40 +64,58 @@ } IrNum = IrData[0]/12; - +d } */ +void Interval(void){ + WaitFlag=0; +} void move(int vr,int vl, double vs ,int Degree){ double pwm[4] = {0}; uint8_t i = 0; - double vias=10.0; - double dif; + double CompassVias=15.0; + double CompassDif; + + vr=vr*( 1.0); + vl=vl*(-1.0); + Degree = Degree*(-1.0); - dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE; - if(abs(dif)<vias){ - - pwm[0] = vr; - pwm[1] = 0; - pwm[2] = 0; - pwm[3] = vl*(1.0)*(-1.0); + CompassDif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE; + if(abs(CompassDif)<CompassVias){ + + + CompassVias=15.0; + + pwm[0] = LastPwm[0] = vr; + pwm[1] = LastPwm[1] =0; + pwm[2] = LastPwm[2] =0; + pwm[3] = LastPwm[3] =vl*(1.0); + + LastDeg = Degree; + + } else{ - if(dif<0){ - //vs = vs - (20-vias); - vs = -(20+(abs(dif))*0.1); - if( abs(dif) >= 100) vs = -(20+(100)*0.1); + CompassVias=10.0; + + if(CompassDif<0){ + //vs = vs - (20-CompassVias); + vs = -(15+(abs(CompassDif))*0.1); + if( abs(CompassDif) >= 100) vs = -(20+(100)*0.1); } else{ - //vs = vs + (20-vias); - vs = (20+(abs(dif))*0.1); - if( abs(dif) >= 100) vs = (20+(100)*0.1); + //vs = vs + (20-CompassVias); + vs = (15+(abs(CompassDif))*0.1); + if( abs(CompassDif) >= 100) vs = (20+(100)*0.1); } pwm[0] = vs; pwm[1] = 0; pwm[2] = 0; pwm[3] = vs*(-1)*(-1); + + Degree = 0.0; } if(Loop==1){ pwm[0] = 0; @@ -105,17 +123,8 @@ pwm[2] = 0; pwm[3] = 0; } - /*if(vs<0){ - pwm[0] = vr - vs*k; - pwm[1] = 0; - pwm[2] = 0; - pwm[3] = vl*1.2 + vs*k; - }else{ - pwm[0] = vr + vs*k; - pwm[1] = 0; - pwm[2] = 0; - pwm[3] = (vl - vs*k)*1.2; - }*/ + + for(i = 0; i < 4; i++){ if(pwm[i] > 100){ pwm[i] = 100; @@ -129,9 +138,22 @@ }else if(Degree < -110){ Degree = -110; } - SetDegree = Degree; + + if(WaitFlag==1) Degree = LastDegree; + + + SetDegree = LastDegree = Degree; S555.position(SetDegree*11.0/9.0); - wait_ms(10); + + if(WaitFlag==0){ + WaitFlag = 1; + STimer.attach(Interval, WAIT_TIME); + } + +} + +void moving(int IrNumber){ + move(vrl[IrNumber],vrl[IrNumber],0,IrServo[IrNumber]); } uint8_t PingChange(uint8_t LineData){ @@ -143,10 +165,10 @@ return 0; } - if(PingData[0] <40) LinePing = LinePing + 1; - if(PingData[1] <40) LinePing = LinePing + 2; - if(PingData[2] <40) LinePing = LinePing + 4; - if(PingData[3] <40) LinePing = LinePing + 8; + if(PingData[0] <20) LinePing = LinePing + 8; + if(PingData[1] <20) LinePing = LinePing + 2; + if(PingData[2] <20) LinePing = LinePing + 4; + if(PingData[3] <20) LinePing = LinePing + 1; if(LinePing&0x01){ @@ -252,7 +274,14 @@ Last_Degree = degree; return ; } - Last_Vector = DegreeToVector[(*Degree-1)/90]; + if(*Degree == 270){ + Last_Vector = DegreeToVector[0]; + *Degree = -90; + *Power = *Power * Last_Vector; + Last_Degree = degree; + return ; + } + Last_Vector = DegreeToVector[(*Degree)/90]; *Degree = Angle[*Degree%180]; *Power = *Power * Last_Vector; Last_Degree = degree; @@ -374,14 +403,123 @@ void GoHome()//Ball is not found. { - - /*if(PingData[2] >=60){//後ろからの距離60cm - move(-20,-20,CompassPID,0); - return ; - } - */ - move(0,0,CompassPID,0);//stop - + static bool State[20]={0}; + /* + State + 0 + 1 + 2 + 3 + 4 + 5 + 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; + + + + if(PingData[2] >=60) State[0]=1; + + if(PingData[2] <=40) State[1]=1; + + if(PingData[2] >=90-10) State[5]=1; + + if(PingData[2] <=40+10) State[6]=1; + + if(PingData[2] >=10) State[7]=1; + + if(PingData[2] <=5) State[8]=1; + + if(PingData[2] >=25-5) State[9]=1; + + if(PingData[2] <=5+5) State[10]=1; + + + + if(PingData[0]>PingData[1]) State[2]=1; + + if( + (PingData[0]+PingData[1]) + >= 120 + ) State[3]=1; + + if( + (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) + <= 10 + ) State[4]=1; + + + /*if(State[0]){ + + move(vrl[6],vrl[6],CompassPID,IrServo[6]); + return ; + }*/ + + 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[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]); + return ; + } + else{ + move(vrl[3],vrl[3],CompassPID,IrServo[3]); + return ; + } + } + } @@ -395,12 +533,6 @@ CompassPID = (pid.compute()); } - - - - - - uint8_t SwRead(){ /****** *retrun : sw_state @@ -472,7 +604,7 @@ CompassDef = (Compass / 10); wait_ms(10); } - pidupdate.attach(&PidUpdate, PID_CYCLE); + //pidupdate.attach(&PidUpdate, PID_CYCLE); } @@ -568,38 +700,46 @@ } } + +uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){ + return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one; +} + int main() { //uint8_t IrNumOld = 0;//過去値 /*Line*/ uint8_t LineData = 0; - uint8_t LinePing = 0; + //uint8_t LinePing = 0; /*State */ - uint8_t LineIr = 0; + /*uint8_t LineIr = 0; uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06, 0x04,0x04,0x0B,0x08,0x08,0x09,0x00}; - + */ /*行動設定*/ - int Power = 0; - int Degree = 0; + //int Power = 0; + //int Degree = 0; /*楽しい変数達*/ - int nDegree =0;//基礎角 - int addDegree = 0;//追加角 + //int nDegree =0;//基礎角 + //int addDegree = 0;//追加角 + /**/ + //int Gap=2; + bool LineState[10]={0}; /*関数ポインタ*/ //void (*AnotherAction[3])(uint8_t [],double); - void (*AnotherAction[3])(); + /*void (*AnotherAction[3])(); AnotherAction[0] = IrFrontAction; AnotherAction[1] = IrBackAction; - AnotherAction[2] = GoHome; + AnotherAction[2] = GoHome;*/ SetUp();/*set up routine*/ @@ -611,94 +751,145 @@ Loop = 0; while(1){ - - S555.calibrate(0.0005, 120.0); - - //Receive(); - //Lcd.printf("%d\n",IrNum); - /*白線を読んでいないか確認する*/ - LineData = (~Line+0x00) & 0x0F; - if(LineData){ - LineIr = LineData & IrChange[IrNum]; - LinePing = PingChange(LineData); - if(LineIr){ - move(0,0,0,0); - while(LineIr){ - Led[1] = Led[2] = Led[3] = 1; - //Receive(); - LineData = (~Line+0x00) & 0x0F; - LineIr = LineData & IrChange[IrNum]; - wait_ms(10); - } - }else if(LinePing){ - move(0,0,0,0); - while(LinePing){ - Led[1] = Led[2] = Led[3] = 1; - //Receive(); - LineData = (~Line+0x00) & 0x0F; - LinePing = PingChange(LineData); - - wait_ms(10); - } - } + if( + ( + ( + ( + (Compass / 10) + 540 - CompassDef + ) % 360 + ) - REFERENCE + ) >15.0 + ){ - - Led[1] = Led[2] = Led[3] = 0; - - } - - Power = 0; - Led[0] = 1; - Degree = 0; - SetC = 0.0; - - - Led[3] = 1; - //Receive(); - Degree = IrDegree(); - - if((Degree == 0)||(Degree == 180)||(IrNum == 12)){ - (AnotherAction[IrNum/6])(); + move(0,0,0,0); continue; } - /* - if(IrNum == 12){ - move(0,0,0,0); - wait_ms(10); - continue; - }*/ + LineData = (~Line+0x00) & 0x0F; + + for(int i=0; i<10; i++) LineState[i]=0; - nDegree = wrapDegree[Degree/15]; - Power = 20; + if(LineData!=0){ + + if(PingData[0]>PingData[1]) LineState[0]=1; + if(PingData[2]<=20) 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]<=5) LineState[2]=1; + if(PingData[1]<=5) LineState[3]=1; + if(PingData[2]<=5) 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(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((LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07)!=0){ + wait(0.2); + continue; + } + } - Degree = nDegree + addDegree; - if((Degree <0)||(Degree>=360)){ - Degree = 0; + + if(PingData[2]>=90){ + + moving(6); + wait(0.2); + continue; } - fool(&Degree,&Power); - move(Power,Power,CompassPID,Degree); - - //wait_ms(500); - Led[0] =0; - wait_ms(10); + else{ + + moving(12); + continue; + } } + /* + Gap=2; - while(0){ - //デモプログラム - //Receive(); - pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]); - //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass); - - //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]); - //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]); - wait(0.1); + 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; + } - - + */ }