CatPot 2015-2016
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
--- 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;
+
}
-
-
+ */
}