RotarySW.h

00001 // Master
00002 
00003 
00004 
00005 // "0"攻撃モード
00006 void FW(void) {
00007     led1=0; led2=0; led3=0; led4=0;
00008     Speed = 1.0;
00009     MBED_IR();
00010     COMPASS();
00011         if (Angle == 001) Speed = 0.0;
00012 
00013     // LINEの制御
00014         if(LINE_R || LINE_L)  Angle = 360 - Angle;
00015         else if(LINE_F)  Angle = 180;
00016         else if(LINE_B)  Angle = 0;
00017 
00018     // Kickerの制御
00019         if(!Kicker_Flag) {                  // Chargeの開始
00020             Kicker_Timer.reset();
00021             shout=0;  charge=1;  Kicker_Timer.start();
00022             Kicker_Flag = 1;
00023         } else {                            // Charge時間を確認
00024             // Kicker_Timer.stop();
00025             if (Kicker_Timer.read() > 10.0) {
00026                 charge = 0;
00027                 Kicker_Flag = 2;
00028             }
00029         }
00030         if(BallCheck && Kicker_Flag==2) {   // Kick可能な時の処理
00031             int ping_width = PING_R + PING_L;
00032             if (ping_width>150) {
00033                 ping_width = PING_R - PING_L;
00034                 if (ping_width> 90) Moter(1.0, Angle,  0.1);
00035                 if (ping_width<-90) Moter(1.0, Angle, -0.1);
00036             }
00037             charge=0;  shout=1;  wait(0.3);
00038             Kicker_Flag = 0;
00039         }
00040     Moter(Speed, Angle, Auto_Corrction());
00041 }
00042 
00043 // "1"防御モード
00044 void DF(void) {
00045     MBED_IR();
00046     MBED_PING();
00047         if (PING_B>25) {
00048             int ping_width = PING_R - PING_L;
00049             if(ping_width> 90) Angle=90;
00050             if(ping_width<-90) Angle=270;
00051         }
00052     COMPASS();
00053     // Kickerの制御
00054         if(!Kicker_Flag) {                  // Chargeの開始
00055             Kicker_Timer.reset();
00056             shout=0;  charge=1;  Kicker_Timer.start();
00057             Kicker_Flag = 1;
00058         } else {                            // Charge時間を確認
00059             // Kicker_Timer.stop();
00060             if (Kicker_Timer.read() > 10.0) {
00061                 charge = 0;
00062                 Kicker_Flag = 2;
00063             }
00064         }
00065         if(BallCheck && Kicker_Flag==2) {   // Kick可能な時の処理
00066             int ping_width = PING_R + PING_L;
00067             if (ping_width>150) {
00068                 ping_width = PING_R - PING_L;
00069                 if (ping_width> 90) Moter(1.0, Angle,  0.1);
00070                 if (ping_width<-90) Moter(1.0, Angle, -0.1);
00071             }
00072             charge=0;  shout=1;  wait(0.3);
00073             Kicker_Flag = 0;
00074         }
00075     // LINEの制御
00076         if(LINE_R || LINE_L)  Angle = 360 - Angle;
00077         else if(LINE_F)  Angle = 180;
00078         else if(LINE_B)  Angle = 0;
00079     Moter(Speed, Angle, Auto_Corrction());
00080     Moter(Speed, Angle, Auto_Corrction());
00081     led1=0; led2=0; led3=0; led4=0;
00082 }
00083 
00084 
00085 
00086 // "2"IRデバッグモード
00087 void DEBUG_IR(void) {
00088     char ir[2];
00089     do val = I2C_mbed.read(mbed_slave, ir, 2); while(val);
00090     // LCDに表示
00091         lcd.locate(0,0);  lcd.printf("near: ");
00092         lcd.locate(0,1);  lcd.printf("long: ");
00093         for (int i = 0; i < 8; ++i) {
00094             lcd.locate(i+6, 0);  lcd.printf("%1d", ir[0]%2);  ir[0] = ir[0] / 2;
00095             lcd.locate(i+6, 1);  lcd.printf("%1d", ir[1]%2);  ir[1] = ir[1] / 2;
00096         }
00097     led1=0; led2=0; led3=0; led4=0;
00098 }
00099 
00100 
00101 // "3"PINGデバッグモード
00102 void DEBUG_PING(void) {
00103     char ping[4];
00104     val = I2C_mbed.read(mbed_slave, ping, 4);
00105     if(!val) led1 = 1;
00106     // LCDに表示
00107         lcd.locate(0,0);  lcd.printf("F:%04.1f  ", ping[0]/10);
00108         lcd.locate(8,0);  lcd.printf("R:%04.1f  ", ping[1]/10);
00109         lcd.locate(0,1);  lcd.printf("L:%04.1f  ", ping[3]/10);
00110         lcd.locate(8,1);  lcd.printf("B:%04.1f  ", ping[2]/10);
00111     led1=0; led2=0; led3=0; led4=0;
00112 }
00113 
00114 // "4"ANGLEデバッグモード
00115 void DEBUG_ANGLE(void) {
00116     COMPASS();
00117     lcd.locate(8, 1);
00118     lcd.printf("%05.1f", Compass);
00119     led1=0; led2=0; led3=0; led4=0;
00120 }
00121 
00122 // "5"KICKERデバッグモード
00123 void DEBUG_KICKER(void) {
00124     shout=0;  charge=1;  wait(3.0);
00125     shout=0;  charge=0;  wait(0.5);
00126     // while(!SW1);
00127     shout=1;  charge=0;  wait(0.5);
00128     led1=0; led2=0; led3=0; led4=0;
00129 }
00130 
00131 // "6"MOTERデバッグモード
00132 void DEBUG_MOTER(void) {
00133     for (int i = 0; i < 4; ++i) {
00134         Moter(1.0, i*90, 0.0);  wait(0.5);
00135         switch(i) {
00136             case 0:
00137                 Moter(1.0, 2*90, 0.0);  wait(0.5);  break;
00138             case 1:
00139                 Moter(1.0, 3*90, 0.0);  wait(0.5);  break;
00140             case 2:
00141                 Moter(1.0, 0*90, 0.0);  wait(0.5);  break;
00142             case 3:
00143                 Moter(1.0, 1*90, 0.0);  wait(0.5);  break;
00144         }
00145     }
00146     led1=0; led2=0; led3=0; led4=0;
00147 }
00148 
00149 // "7"LINEデバッグモード
00150 void DEBUG_LINE(void) {
00151     // LCDに表示
00152         lcd.locate(0,0);  lcd.printf("F:%1d     ", (LINE_F)?1:0);
00153         lcd.locate(8,0);  lcd.printf("R:%1d     ", (LINE_R)?1:0);
00154         lcd.locate(0,1);  lcd.printf("L:%1d     ", (LINE_L)?1:0);
00155         lcd.locate(8,1);  lcd.printf("B:%1d     ", (LINE_B)?1:0);
00156     led1=0; led2=0; led3=0; led4=0;
00157 }
00158 
00159 
00160 
00161 void (*Debug_Mode[])(void) = {FW, DF, DEBUG_IR, DEBUG_PING, DEBUG_ANGLE, DEBUG_KICKER, DEBUG_MOTER, DEBUG_LINE};
00162 
00163 
00164 
00165 
00166 
00167 
00168 
00169 
00170 
00171 
00172