CatPot 2015-2016
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:
- 6:2c9cb40ada59
- Parent:
- 4:7c488c059498
File content as of revision 6:2c9cb40ada59:
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;
}