CatPot 2015-2016
version 3 通信方式，マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
Diff: main.cpp
- Revision:
- 6:bfc0e8d2d3ae
- Parent:
- 5:09afcbe0c18f
--- 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);