version 3 通信方式,マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
Diff: main.cpp
- Revision:
- 6:bfc0e8d2d3ae
- Parent:
- 5:09afcbe0c18f
diff -r 09afcbe0c18f -r bfc0e8d2d3ae main.cpp --- a/main.cpp Fri Mar 13 01:14:38 2015 +0000 +++ b/main.cpp Fri Mar 13 12:26:29 2015 +0000 @@ -50,6 +50,9 @@ void ServoWait(){ WaitFlag = 0; } +void GoHinit(){ + GoHFlag = 0; +} void move(int vr,int vl, double vs ,int Degree){ @@ -62,7 +65,6 @@ dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE; if(abs(dif)<vias){ - pwm[0] = vr; pwm[1] = 0; pwm[2] = 0; @@ -126,6 +128,7 @@ static uint8_t Last_Line; static uint8_t Last_Ping; uint8_t LinePing = 0; + uint8_t ReData =0; if(!LineData){ return 0; @@ -139,32 +142,29 @@ if(LinePing&0x01){ if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){ - Last_Ping = LinePing; - Last_Line = LineData; - return 1; + ReData += 1; } } if(LinePing&0x02){ if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){ - Last_Ping = LinePing; - Last_Line = LineData; - return 2; + ReData += 2; } } if(LinePing&0x04){ if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){ - Last_Ping = LinePing; - Last_Line = LineData; - return 4; + ReData += 4; } } if(LinePing&0x08){ if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){ - Last_Ping = LinePing; - Last_Line = LineData; - return 8; + ReData += 8; } } + if(ReData){ + Last_Ping = LinePing; + Last_Line = LineData; + return ReData; + } Last_Ping = 0; Last_Line = 0; return 0; @@ -291,34 +291,49 @@ void IrFrontAction()//ball 12 or 0 o-clock { - if(IrData[1]>70){ + if(IrData[1]>65){ move(30,30,CompassPID,0); return; } - if(IrData[1]>60){ + if(IrData[1]>57){ move(20,20,CompassPID,0); return; } /*IrData[1]>500*/ move(10,10,0,0); - if(PingData[0]<50){ + if(PingData[0]<60){ if((PingData[1]<60)&&(PingData[1]>40)){ /*右側に居る*/ + if(IrData[1]<50){ + move(40,25,0,10); + Kick = 1; + wait_ms(200); + move(30,30,0,0); + Kick = 0; + return; + } + move(40,25,0,10); - Kick = 1; wait_ms(200); move(30,30,0,0); - Kick = 0; return; + } if((PingData[1]<60)&&(PingData[1]>40)){ /*左側に居る*/ + if(IrData[1]<50){ + move(25,40,0,-10); + Kick = 1; + wait_ms(200); + move(30,30,0,0); + Kick = 0; + return; + } move(25,40,0,-10); - Kick = 1; wait_ms(200); move(30,30,0,0); - Kick = 0; return; + } /*それ以外*/ @@ -342,30 +357,59 @@ * 6時にボールがある場合の処理.右と左のスペースを確認して広い方から回り込む * **/ - if(PingData[1]>PingData[3]){ - /*右が大きい*/ - if(IrData[1]>70){ - move(-20,-20,CompassPID,45); + int Pingrl = PingData[1] + PingData[3]; + if(Pingrl >=120){ + if(PingData[1]>PingData[3]){ + /*右が大きい*/ + if(IrData[1]>65){ + move(-25,-25,CompassPID,45); + return; + } + if(IrData[1]>55){ + move(-20,-20,CompassPID,60); + return; + } + move(-20,-20,CompassPID,90); return; } - if(IrData[1]>60){ - move(-20,-20,CompassPID,60); + /*左が大きい*/ + + if(IrData[1]>65){ + move(-25,-25,CompassPID,-45); + return; + } + if(IrData[1]>55){ + move(-20,-20,CompassPID,-60); + return; + } + move(-20,-20,CompassPID,-90); + return; + } + + if(PingData[1]>PingData[3]){ + /*右が大きい*/ + if(IrData[1]>65){ + move(-25,-25,0,30); + return; + } + if(IrData[1]>55){ + move(-20,-20,0,60); return; } - move(-20,-20,CompassPID,80); + move(-15,-15,0,70); return; } /*左が大きい*/ - if(IrData[1]>70){ - move(-20,-20,CompassPID,-45); + if(IrData[1]>65){ + move(-25,-25,CompassPID,-30); return; } - if(IrData[1]>60){ + if(IrData[1]>55){ move(-20,-20,CompassPID,-60); return; } - move(-20,-20,CompassPID,-80); + move(-15,-15,CompassPID,-70); return; } @@ -374,25 +418,32 @@ { int pingRl =0; int PowLev; + if(!GoHFlag){ + GoHFlag = 1; + GoHTimer.attach(&GoHinit,1.0); + move(0,0,0,0); + wait_ms(100); + + } if(PingData[2] >=50){//後ろからの距離60cm - move(-25,-25,CompassPID,0); + move(-25,-25,0,0); return ; } - if(PingData[2] <= 30){ - move(17,17,0,0); + if(PingData[2] <= 25){ + move(15,15,0,0); return ; } pingRl = PingData[1]+PingData[3]; - if(pingRl <100){ - move(0,0,CompassPID,0);//stop + if(pingRl <120){ + move(0,0,0,0);//stop return; } - if(PingData[1] < 52){ - PowLev = (52 - PingData[1])*1.3; + if(PingData[1] < 70){ + PowLev = (70 - PingData[1]); move(PowLev,PowLev,0,90); return ; - }else if (PingData[3] < 52){ - PowLev = (52 - PingData[3])*1.35; + }else if (PingData[3] < 70){ + PowLev = (70 - PingData[3]); move(PowLev,PowLev,0,-90); return ; } @@ -526,7 +577,7 @@ if(State == Debug2){ while((State == Debug2)){ //Receive(); - Lcd.printf("%d\n",IrNum); + Lcd.printf("%d\t%d\n",IrNum,IrData[1]); wait_ms(100); State = SwRead(); } @@ -621,10 +672,6 @@ if(LineData){ LineIr = LineData & IrChange[IrNum]; LinePing = PingChange(LineData); - if(LinePing){ - move(0,0,0,0); - wait_ms(200); - } if(LineIr){ move(0,0,0,0); StopCt = 0; @@ -635,7 +682,7 @@ } StopCt++; Led[1] = Led[2] = Led[3] = 1; - //Receive(); + LineData = (~Line+0x00) & 0x0F; LineIr = LineData & IrChange[IrNum]; wait_ms(10); @@ -646,11 +693,19 @@ while(LinePing){ if(StopCt>30){ GoHome(); - wait_ms(200); + wait_ms(300); } StopCt++; Led[1] = Led[2] = Led[3] = 1; - //Receive(); + + if((LinePing&0x06)||(LinePing&0x0C)){ + if(LinePing&0x06){ + move(15,15,0,45); + }else if(LinePing&0x0C){ + move(15,15,0,-45); + } + wait_ms(200); + } LineData = (~Line+0x00) & 0x0F; LinePing = PingChange(LineData); @@ -686,95 +741,113 @@ }*/ nDegree = wrapDegree[Degree/15]; - Power = 30; + Power = 35; + + if((Degree>30)&&(Degree<75)){ + if(IrData[1] < 56){ + if(Degree>180){/*へらす*/ + addDegree = -10; + }else{ + addDegree = 10; + } + } + } Temp = Degree%180; - if((Temp>70)&&(Temp<110)){ + if((Degree==75)){ + if(IrData[1]<55){ + + } + } + if((Temp>80)&&(Temp<100)){ if(PingData[2]<40){ - if(IrData[1] > 70){ - if(Degree/180){ - addDegree = 90; - }else{ - addDegree = -90; - } - }else if(IrData[1] >60){ - if(Degree/180){ - addDegree = 60; + if(Degree>180){ + addDegree = 100; + }else{ + addDegree = -100; + } + + }else{ + if(IrData[1] > 65){ + if(Degree>180){ + addDegree = 40; }else{ - addDegree = -60; - } - }else{ - addDegree = 0; - } - }else{ - if(IrData[1] > 70){ - if(Degree/180){ - addDegree = 30; - - }else{ - - addDegree = -30; + addDegree = -40; } - }else if(IrData[1] >60){ - if(Degree/180){ - addDegree = 20; - - }else{ - addDegree = -20; - } - - }else { addDegree = 0; } } - }else{ - if(IrData[1] > 70){ - if(Degree/180){ - addDegree = 30; + } + /*if(IrData[1] > 65){ + if(Degree>180){ + addDegree = 50; }else{ - addDegree = -30; + addDegree = -50; } - }else if(IrData[1] >60){ - if(Degree/180){ - addDegree = 20; + }else if(IrData[1] >55){ + if(Degree>180){ + addDegree = 30; }else{ - addDegree = -20; + addDegree = -30; } }else { addDegree = 0; - } - } + }*/ + if(PingData[2]<40){ - if((Degree>150)&&(Degree<300)) - if(Degree/180){ - addDegree = 60; + if((Degree>=150)&&(Degree<=300)){ + if(Degree>180){ + addDegree = 90; }else{ - addDegree = -60; + addDegree = -90; + } + } + }else if(PingData[2]<70){ + if((Degree>=150)&&(Degree<=300)){ + if(Degree>180){ + addDegree = 40; + + }else{ + + addDegree = -40; + } + } + } + if(((Degree>=0)&&(Degree<=30))||((Degree>=330)&&(Degree<360))){ + if(IrData[1]<55){ + Power = 15; } } - + + if(((Degree>30)&&(Degree<75))||((Degree>315)&&(Degree<330))){ + if(IrData[1]<55){ + Power = 32; + } + } Degree = nDegree + addDegree; if((Degree <0)||(Degree>=360)){ Degree = 0; } + //Lcd.printf("%d\n",Degree); fool(&Degree,&Power); - move(Power,Power,CompassPID,Degree); + move(Power,Power,0,Degree); //wait_ms(500); + //Lcd.cls(); Led[0] =0; wait_ms(10);