File content as of revision 0:27bf77b6ec71:

// 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};