Diff: RotarySW.h

Revision:

0:27bf77b6ec71

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RotarySW.h	Mon Mar 10 07:56:41 2014 +0000
@@ -0,0 +1,172 @@
+// Master
+
+
+
+// "0"攻撃モード
+void FW(void) {
+    led1=0; led2=0; led3=0; led4=0;
+    Speed = 1.0;
+    MBED_IR();
+    COMPASS();
+        if (Angle == 001) Speed = 0.0;
+
+    // LINEの制御
+        if(LINE_R || LINE_L)  Angle = 360 - Angle;
+        else if(LINE_F)  Angle = 180;
+        else if(LINE_B)  Angle = 0;
+
+    // Kickerの制御
+        if(!Kicker_Flag) {                  // Chargeの開始
+            Kicker_Timer.reset();
+            shout=0;  charge=1;  Kicker_Timer.start();
+            Kicker_Flag = 1;
+        } else {                            // Charge時間を確認
+            // Kicker_Timer.stop();
+            if (Kicker_Timer.read() > 10.0) {
+                charge = 0;
+                Kicker_Flag = 2;
+            }
+        }
+        if(BallCheck && Kicker_Flag==2) {   // Kick可能な時の処理
+            int ping_width = PING_R + PING_L;
+            if (ping_width>150) {
+                ping_width = PING_R - PING_L;
+                if (ping_width> 90) Moter(1.0, Angle,  0.1);
+                if (ping_width<-90) Moter(1.0, Angle, -0.1);
+            }
+            charge=0;  shout=1;  wait(0.3);
+            Kicker_Flag = 0;
+        }
+    Moter(Speed, Angle, Auto_Corrction());
+}
+
+// "1"防御モード
+void DF(void) {
+    MBED_IR();
+    MBED_PING();
+        if (PING_B>25) {
+            int ping_width = PING_R - PING_L;
+            if(ping_width> 90) Angle=90;
+            if(ping_width<-90) Angle=270;
+        }
+    COMPASS();
+    // Kickerの制御
+        if(!Kicker_Flag) {                  // Chargeの開始
+            Kicker_Timer.reset();
+            shout=0;  charge=1;  Kicker_Timer.start();
+            Kicker_Flag = 1;
+        } else {                            // Charge時間を確認
+            // Kicker_Timer.stop();
+            if (Kicker_Timer.read() > 10.0) {
+                charge = 0;
+                Kicker_Flag = 2;
+            }
+        }
+        if(BallCheck && Kicker_Flag==2) {   // Kick可能な時の処理
+            int ping_width = PING_R + PING_L;
+            if (ping_width>150) {
+                ping_width = PING_R - PING_L;
+                if (ping_width> 90) Moter(1.0, Angle,  0.1);
+                if (ping_width<-90) Moter(1.0, Angle, -0.1);
+            }
+            charge=0;  shout=1;  wait(0.3);
+            Kicker_Flag = 0;
+        }
+    // LINEの制御
+        if(LINE_R || LINE_L)  Angle = 360 - Angle;
+        else if(LINE_F)  Angle = 180;
+        else if(LINE_B)  Angle = 0;
+    Moter(Speed, Angle, Auto_Corrction());
+    Moter(Speed, Angle, Auto_Corrction());
+    led1=0; led2=0; led3=0; led4=0;
+}
+
+
+
+// "2"IRデバッグモード
+void DEBUG_IR(void) {
+    char ir[2];
+    do val = I2C_mbed.read(mbed_slave, ir, 2); while(val);
+    // LCDに表示
+        lcd.locate(0,0);  lcd.printf("near: ");
+        lcd.locate(0,1);  lcd.printf("long: ");
+        for (int i = 0; i < 8; ++i) {
+            lcd.locate(i+6, 0);  lcd.printf("%1d", ir[0]%2);  ir[0] = ir[0] / 2;
+            lcd.locate(i+6, 1);  lcd.printf("%1d", ir[1]%2);  ir[1] = ir[1] / 2;
+        }
+    led1=0; led2=0; led3=0; led4=0;
+}
+
+
+// "3"PINGデバッグモード
+void DEBUG_PING(void) {
+    char ping[4];
+    val = I2C_mbed.read(mbed_slave, ping, 4);
+    if(!val) led1 = 1;
+    // LCDに表示
+        lcd.locate(0,0);  lcd.printf("F:%04.1f  ", ping[0]/10);
+        lcd.locate(8,0);  lcd.printf("R:%04.1f  ", ping[1]/10);
+        lcd.locate(0,1);  lcd.printf("L:%04.1f  ", ping[3]/10);
+        lcd.locate(8,1);  lcd.printf("B:%04.1f  ", ping[2]/10);
+    led1=0; led2=0; led3=0; led4=0;
+}
+
+// "4"ANGLEデバッグモード
+void DEBUG_ANGLE(void) {
+    COMPASS();
+    lcd.locate(8, 1);
+    lcd.printf("%05.1f", Compass);
+    led1=0; led2=0; led3=0; led4=0;
+}
+
+// "5"KICKERデバッグモード
+void DEBUG_KICKER(void) {
+    shout=0;  charge=1;  wait(3.0);
+    shout=0;  charge=0;  wait(0.5);
+    // while(!SW1);
+    shout=1;  charge=0;  wait(0.5);
+    led1=0; led2=0; led3=0; led4=0;
+}
+
+// "6"MOTERデバッグモード
+void DEBUG_MOTER(void) {
+    for (int i = 0; i < 4; ++i) {
+        Moter(1.0, i*90, 0.0);  wait(0.5);
+        switch(i) {
+            case 0:
+                Moter(1.0, 2*90, 0.0);  wait(0.5);  break;
+            case 1:
+                Moter(1.0, 3*90, 0.0);  wait(0.5);  break;
+            case 2:
+                Moter(1.0, 0*90, 0.0);  wait(0.5);  break;
+            case 3:
+                Moter(1.0, 1*90, 0.0);  wait(0.5);  break;
+        }
+    }
+    led1=0; led2=0; led3=0; led4=0;
+}
+
+// "7"LINEデバッグモード
+void DEBUG_LINE(void) {
+    // LCDに表示
+        lcd.locate(0,0);  lcd.printf("F:%1d     ", (LINE_F)?1:0);
+        lcd.locate(8,0);  lcd.printf("R:%1d     ", (LINE_R)?1:0);
+        lcd.locate(0,1);  lcd.printf("L:%1d     ", (LINE_L)?1:0);
+        lcd.locate(8,1);  lcd.printf("B:%1d     ", (LINE_B)?1:0);
+    led1=0; led2=0; led3=0; led4=0;
+}
+
+
+
+void (*Debug_Mode[])(void) = {FW, DF, DEBUG_IR, DEBUG_PING, DEBUG_ANGLE, DEBUG_KICKER, DEBUG_MOTER, DEBUG_LINE};
+
+
+
+
+
+
+
+
+
+
+