Naoya Muramatsu / Mbed 2 deprecated ROBOT_v01

ロボカップのブロック大会2014で使用したプログラムです。 ロボットには、mbedを2台使ってI2C通信しています。 これはMaster側です。 ※独自規格を使用しています。

Dependencies:   ACM1602NI Ping mbed-rtos mbed

RotarySW.h@0:27bf77b6ec71, 2014-03-10 (annotated)

Committer:
denden
Date:
Mon Mar 10 07:56:41 2014 +0000
Revision:
0:27bf77b6ec71
????????????2014?????????????; ???????mbed?2????I2C????????; ???Master????; ; ??????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
denden 0:27bf77b6ec71 1 // Master
denden 0:27bf77b6ec71 2
denden 0:27bf77b6ec71 3
denden 0:27bf77b6ec71 4
denden 0:27bf77b6ec71 5 // "0"攻撃モード
denden 0:27bf77b6ec71 6 void FW(void) {
denden 0:27bf77b6ec71 7 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 8 Speed = 1.0;
denden 0:27bf77b6ec71 9 MBED_IR();
denden 0:27bf77b6ec71 10 COMPASS();
denden 0:27bf77b6ec71 11 if (Angle == 001) Speed = 0.0;
denden 0:27bf77b6ec71 12
denden 0:27bf77b6ec71 13 // LINEの制御
denden 0:27bf77b6ec71 14 if(LINE_R || LINE_L) Angle = 360 - Angle;
denden 0:27bf77b6ec71 15 else if(LINE_F) Angle = 180;
denden 0:27bf77b6ec71 16 else if(LINE_B) Angle = 0;
denden 0:27bf77b6ec71 17
denden 0:27bf77b6ec71 18 // Kickerの制御
denden 0:27bf77b6ec71 19 if(!Kicker_Flag) { // Chargeの開始
denden 0:27bf77b6ec71 20 Kicker_Timer.reset();
denden 0:27bf77b6ec71 21 shout=0; charge=1; Kicker_Timer.start();
denden 0:27bf77b6ec71 22 Kicker_Flag = 1;
denden 0:27bf77b6ec71 23 } else { // Charge時間を確認
denden 0:27bf77b6ec71 24 // Kicker_Timer.stop();
denden 0:27bf77b6ec71 25 if (Kicker_Timer.read() > 10.0) {
denden 0:27bf77b6ec71 26 charge = 0;
denden 0:27bf77b6ec71 27 Kicker_Flag = 2;
denden 0:27bf77b6ec71 28 }
denden 0:27bf77b6ec71 29 }
denden 0:27bf77b6ec71 30 if(BallCheck && Kicker_Flag==2) { // Kick可能な時の処理
denden 0:27bf77b6ec71 31 int ping_width = PING_R + PING_L;
denden 0:27bf77b6ec71 32 if (ping_width>150) {
denden 0:27bf77b6ec71 33 ping_width = PING_R - PING_L;
denden 0:27bf77b6ec71 34 if (ping_width> 90) Moter(1.0, Angle, 0.1);
denden 0:27bf77b6ec71 35 if (ping_width<-90) Moter(1.0, Angle, -0.1);
denden 0:27bf77b6ec71 36 }
denden 0:27bf77b6ec71 37 charge=0; shout=1; wait(0.3);
denden 0:27bf77b6ec71 38 Kicker_Flag = 0;
denden 0:27bf77b6ec71 39 }
denden 0:27bf77b6ec71 40 Moter(Speed, Angle, Auto_Corrction());
denden 0:27bf77b6ec71 41 }
denden 0:27bf77b6ec71 42
denden 0:27bf77b6ec71 43 // "1"防御モード
denden 0:27bf77b6ec71 44 void DF(void) {
denden 0:27bf77b6ec71 45 MBED_IR();
denden 0:27bf77b6ec71 46 MBED_PING();
denden 0:27bf77b6ec71 47 if (PING_B>25) {
denden 0:27bf77b6ec71 48 int ping_width = PING_R - PING_L;
denden 0:27bf77b6ec71 49 if(ping_width> 90) Angle=90;
denden 0:27bf77b6ec71 50 if(ping_width<-90) Angle=270;
denden 0:27bf77b6ec71 51 }
denden 0:27bf77b6ec71 52 COMPASS();
denden 0:27bf77b6ec71 53 // Kickerの制御
denden 0:27bf77b6ec71 54 if(!Kicker_Flag) { // Chargeの開始
denden 0:27bf77b6ec71 55 Kicker_Timer.reset();
denden 0:27bf77b6ec71 56 shout=0; charge=1; Kicker_Timer.start();
denden 0:27bf77b6ec71 57 Kicker_Flag = 1;
denden 0:27bf77b6ec71 58 } else { // Charge時間を確認
denden 0:27bf77b6ec71 59 // Kicker_Timer.stop();
denden 0:27bf77b6ec71 60 if (Kicker_Timer.read() > 10.0) {
denden 0:27bf77b6ec71 61 charge = 0;
denden 0:27bf77b6ec71 62 Kicker_Flag = 2;
denden 0:27bf77b6ec71 63 }
denden 0:27bf77b6ec71 64 }
denden 0:27bf77b6ec71 65 if(BallCheck && Kicker_Flag==2) { // Kick可能な時の処理
denden 0:27bf77b6ec71 66 int ping_width = PING_R + PING_L;
denden 0:27bf77b6ec71 67 if (ping_width>150) {
denden 0:27bf77b6ec71 68 ping_width = PING_R - PING_L;
denden 0:27bf77b6ec71 69 if (ping_width> 90) Moter(1.0, Angle, 0.1);
denden 0:27bf77b6ec71 70 if (ping_width<-90) Moter(1.0, Angle, -0.1);
denden 0:27bf77b6ec71 71 }
denden 0:27bf77b6ec71 72 charge=0; shout=1; wait(0.3);
denden 0:27bf77b6ec71 73 Kicker_Flag = 0;
denden 0:27bf77b6ec71 74 }
denden 0:27bf77b6ec71 75 // LINEの制御
denden 0:27bf77b6ec71 76 if(LINE_R || LINE_L) Angle = 360 - Angle;
denden 0:27bf77b6ec71 77 else if(LINE_F) Angle = 180;
denden 0:27bf77b6ec71 78 else if(LINE_B) Angle = 0;
denden 0:27bf77b6ec71 79 Moter(Speed, Angle, Auto_Corrction());
denden 0:27bf77b6ec71 80 Moter(Speed, Angle, Auto_Corrction());
denden 0:27bf77b6ec71 81 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 82 }
denden 0:27bf77b6ec71 83
denden 0:27bf77b6ec71 84
denden 0:27bf77b6ec71 85
denden 0:27bf77b6ec71 86 // "2"IRデバッグモード
denden 0:27bf77b6ec71 87 void DEBUG_IR(void) {
denden 0:27bf77b6ec71 88 char ir[2];
denden 0:27bf77b6ec71 89 do val = I2C_mbed.read(mbed_slave, ir, 2); while(val);
denden 0:27bf77b6ec71 90 // LCDに表示
denden 0:27bf77b6ec71 91 lcd.locate(0,0); lcd.printf("near: ");
denden 0:27bf77b6ec71 92 lcd.locate(0,1); lcd.printf("long: ");
denden 0:27bf77b6ec71 93 for (int i = 0; i < 8; ++i) {
denden 0:27bf77b6ec71 94 lcd.locate(i+6, 0); lcd.printf("%1d", ir[0]%2); ir[0] = ir[0] / 2;
denden 0:27bf77b6ec71 95 lcd.locate(i+6, 1); lcd.printf("%1d", ir[1]%2); ir[1] = ir[1] / 2;
denden 0:27bf77b6ec71 96 }
denden 0:27bf77b6ec71 97 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 98 }
denden 0:27bf77b6ec71 99
denden 0:27bf77b6ec71 100
denden 0:27bf77b6ec71 101 // "3"PINGデバッグモード
denden 0:27bf77b6ec71 102 void DEBUG_PING(void) {
denden 0:27bf77b6ec71 103 char ping[4];
denden 0:27bf77b6ec71 104 val = I2C_mbed.read(mbed_slave, ping, 4);
denden 0:27bf77b6ec71 105 if(!val) led1 = 1;
denden 0:27bf77b6ec71 106 // LCDに表示
denden 0:27bf77b6ec71 107 lcd.locate(0,0); lcd.printf("F:%04.1f ", ping[0]/10);
denden 0:27bf77b6ec71 108 lcd.locate(8,0); lcd.printf("R:%04.1f ", ping[1]/10);
denden 0:27bf77b6ec71 109 lcd.locate(0,1); lcd.printf("L:%04.1f ", ping[3]/10);
denden 0:27bf77b6ec71 110 lcd.locate(8,1); lcd.printf("B:%04.1f ", ping[2]/10);
denden 0:27bf77b6ec71 111 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 112 }
denden 0:27bf77b6ec71 113
denden 0:27bf77b6ec71 114 // "4"ANGLEデバッグモード
denden 0:27bf77b6ec71 115 void DEBUG_ANGLE(void) {
denden 0:27bf77b6ec71 116 COMPASS();
denden 0:27bf77b6ec71 117 lcd.locate(8, 1);
denden 0:27bf77b6ec71 118 lcd.printf("%05.1f", Compass);
denden 0:27bf77b6ec71 119 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 120 }
denden 0:27bf77b6ec71 121
denden 0:27bf77b6ec71 122 // "5"KICKERデバッグモード
denden 0:27bf77b6ec71 123 void DEBUG_KICKER(void) {
denden 0:27bf77b6ec71 124 shout=0; charge=1; wait(3.0);
denden 0:27bf77b6ec71 125 shout=0; charge=0; wait(0.5);
denden 0:27bf77b6ec71 126 // while(!SW1);
denden 0:27bf77b6ec71 127 shout=1; charge=0; wait(0.5);
denden 0:27bf77b6ec71 128 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 129 }
denden 0:27bf77b6ec71 130
denden 0:27bf77b6ec71 131 // "6"MOTERデバッグモード
denden 0:27bf77b6ec71 132 void DEBUG_MOTER(void) {
denden 0:27bf77b6ec71 133 for (int i = 0; i < 4; ++i) {
denden 0:27bf77b6ec71 134 Moter(1.0, i*90, 0.0); wait(0.5);
denden 0:27bf77b6ec71 135 switch(i) {
denden 0:27bf77b6ec71 136 case 0:
denden 0:27bf77b6ec71 137 Moter(1.0, 2*90, 0.0); wait(0.5); break;
denden 0:27bf77b6ec71 138 case 1:
denden 0:27bf77b6ec71 139 Moter(1.0, 3*90, 0.0); wait(0.5); break;
denden 0:27bf77b6ec71 140 case 2:
denden 0:27bf77b6ec71 141 Moter(1.0, 0*90, 0.0); wait(0.5); break;
denden 0:27bf77b6ec71 142 case 3:
denden 0:27bf77b6ec71 143 Moter(1.0, 1*90, 0.0); wait(0.5); break;
denden 0:27bf77b6ec71 144 }
denden 0:27bf77b6ec71 145 }
denden 0:27bf77b6ec71 146 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 147 }
denden 0:27bf77b6ec71 148
denden 0:27bf77b6ec71 149 // "7"LINEデバッグモード
denden 0:27bf77b6ec71 150 void DEBUG_LINE(void) {
denden 0:27bf77b6ec71 151 // LCDに表示
denden 0:27bf77b6ec71 152 lcd.locate(0,0); lcd.printf("F:%1d ", (LINE_F)?1:0);
denden 0:27bf77b6ec71 153 lcd.locate(8,0); lcd.printf("R:%1d ", (LINE_R)?1:0);
denden 0:27bf77b6ec71 154 lcd.locate(0,1); lcd.printf("L:%1d ", (LINE_L)?1:0);
denden 0:27bf77b6ec71 155 lcd.locate(8,1); lcd.printf("B:%1d ", (LINE_B)?1:0);
denden 0:27bf77b6ec71 156 led1=0; led2=0; led3=0; led4=0;
denden 0:27bf77b6ec71 157 }
denden 0:27bf77b6ec71 158
denden 0:27bf77b6ec71 159
denden 0:27bf77b6ec71 160
denden 0:27bf77b6ec71 161 void (*Debug_Mode[])(void) = {FW, DF, DEBUG_IR, DEBUG_PING, DEBUG_ANGLE, DEBUG_KICKER, DEBUG_MOTER, DEBUG_LINE};
denden 0:27bf77b6ec71 162
denden 0:27bf77b6ec71 163
denden 0:27bf77b6ec71 164
denden 0:27bf77b6ec71 165
denden 0:27bf77b6ec71 166
denden 0:27bf77b6ec71 167
denden 0:27bf77b6ec71 168
denden 0:27bf77b6ec71 169
denden 0:27bf77b6ec71 170
denden 0:27bf77b6ec71 171
denden 0:27bf77b6ec71 172