ロボカップのブロック大会2014で使用したプログラムです。 ロボットには、mbedを2台使ってI2C通信しています。 これはMaster側です。 ※独自規格を使用しています。
Dependencies: ACM1602NI Ping mbed-rtos mbed
RotarySW.h
- Committer:
- denden
- Date:
- 2014-03-10
- Revision:
- 0:27bf77b6ec71
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};