CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
Loop.txt
- Committer:
- lilac0112_1
- Date:
- 2015-03-14
- Revision:
- 7:81f57b67dff8
- Parent:
- 6:2c9cb40ada59
File content as of revision 7:81f57b67dff8:
while(0){ //S555.calibrate(0.0006, 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,LastDegree); 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,LastDegree); while(LinePing){ Led[1] = Led[2] = Led[3] = 1; //Receive(); LineData = (~Line+0x00) & 0x0F; LinePing = PingChange(LineData); wait_ms(10); } } 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])(); continue; }*/ if(IrNum == 12){ (AnotherAction[2])();//GoHome(); //move(0,0,0,LastDegree); wait_ms(10); //continue; } /* nDegree = wrapDegree[Degree/15]; Power = 20; */ // Degree = nDegree + addDegree; if((Degree <0)||(Degree>=360)){ Degree = 0; } //fool(&Degree,&Power); move(Power,Power,CompassPID,Degree); //wait_ms(500); Led[0] =0; wait_ms(10); } 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]); /*for(int i=0; i<13; i++){ move(vrl[i],vrl[i],0,IrServo[i]); wait(0.5); }*/ /*if(PingData[2]>=40){ move(vrl[6],vrl[6],0,IrServo[6]); }*/ /* if( ( ( ( (Compass / 10) + 540 - CompassDef ) % 360 ) - REFERENCE ) >15.0 ){ move(0,0,0,0); continue; } if(PingData[2]>=40){ move(vrl[6],vrl[6],0,IrServo[6]); continue; } GoHome();*/ } while(1){ //デモプログラム //ping continue; //line continue; LineData = (~Line+0x00) & 0x0F; if(Line!=0){ if(LastPwm[0]==0){ } move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg); wait(0.5); continue; } /* if(((((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE) >15.0){ move(0,0,0,0); continue; } if(PingData[2]>=40){ move(vrl[6],vrl[6],0,IrServo[6]); continue; } */ /*if(PingData[2]>=40){ move(vrl[6],vrl[6],0,IrServo[6]); continue; }*/ 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; } } while(0){ moving(6); } /*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; }