yusuke takahashi / Mbed 2 deprecated Ball_fanction

IRIR

main.cpp@2:22b099fbdd14, 2013-03-08 (annotated)

Committer:
yusuke_robocup
Date:
Fri Mar 08 03:49:58 2013 +0000
Revision:
2:22b099fbdd14
Parent:
1:d6c8be12a3de 
Ball_fanction4

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusuke_robocup 0:0c7c6eefafe4 1 #include "mbed.h"
yusuke_robocup 0:0c7c6eefafe4 2
yusuke_robocup 0:0c7c6eefafe4 3 #define IR_TIME_NOTFOUND 833 /* 見つけられなかったと判断するまでの時間(単位：us) */
yusuke_robocup 0:0c7c6eefafe4 4 #define IR_COUNTMAX 487 /*最大パルス幅 パルスの存在しうる最大時間は487us*/
yusuke_robocup 1:d6c8be12a3de 5 #define ALL_IR 9
yusuke_robocup 2:22b099fbdd14 6 #define DIREC_IR 8
yusuke_robocup 0:0c7c6eefafe4 7 #define DIRECTION 16
yusuke_robocup 0:0c7c6eefafe4 8 #define TERM 0
yusuke_robocup 2:22b099fbdd14 9 #define DELTA -2
yusuke_robocup 0:0c7c6eefafe4 10 #define SWAP(type,a,b) { type temp = a; a = b; b = temp; }
yusuke_robocup 0:0c7c6eefafe4 11
yusuke_robocup 0:0c7c6eefafe4 12 Serial pc(USBTX, USBRX); // tx, rx
yusuke_robocup 0:0c7c6eefafe4 13
yusuke_robocup 0:0c7c6eefafe4 14 Timer timer_ir; /* 赤外線用タイマー */
yusuke_robocup 0:0c7c6eefafe4 15
yusuke_robocup 0:0c7c6eefafe4 16 BusOut myleds(p18, p17, p16, p15, p14, p13, p12, p11); //出力用ポート
yusuke_robocup 0:0c7c6eefafe4 17
yusuke_robocup 0:0c7c6eefafe4 18 BusOut mbedleds(LED4,LED3,LED2,LED1);
yusuke_robocup 0:0c7c6eefafe4 19
yusuke_robocup 0:0c7c6eefafe4 20 /* 赤外線センサに使うpinを配列に格納 */
yusuke_robocup 0:0c7c6eefafe4 21 PinName ir_num[ALL_IR] = {
yusuke_robocup 0:0c7c6eefafe4 22 p13,
yusuke_robocup 0:0c7c6eefafe4 23 p14,
yusuke_robocup 0:0c7c6eefafe4 24 p15,
yusuke_robocup 0:0c7c6eefafe4 25 p16,
yusuke_robocup 0:0c7c6eefafe4 26 p17,
yusuke_robocup 0:0c7c6eefafe4 27 p18,
yusuke_robocup 0:0c7c6eefafe4 28 p19,
yusuke_robocup 1:d6c8be12a3de 29 p20,
yusuke_robocup 1:d6c8be12a3de 30 p30
yusuke_robocup 0:0c7c6eefafe4 31 };
yusuke_robocup 0:0c7c6eefafe4 32
yusuke_robocup 0:0c7c6eefafe4 33 int Convert_Direction[DIRECTION] = {
yusuke_robocup 1:d6c8be12a3de 34 90,
yusuke_robocup 1:d6c8be12a3de 35 67,
yusuke_robocup 0:0c7c6eefafe4 36 45,
yusuke_robocup 1:d6c8be12a3de 37 22,
yusuke_robocup 1:d6c8be12a3de 38 0,
yusuke_robocup 1:d6c8be12a3de 39 337,
yusuke_robocup 1:d6c8be12a3de 40 315,
yusuke_robocup 1:d6c8be12a3de 41 292,
yusuke_robocup 1:d6c8be12a3de 42 270,
yusuke_robocup 1:d6c8be12a3de 43 247,
yusuke_robocup 1:d6c8be12a3de 44 225,
yusuke_robocup 1:d6c8be12a3de 45 202,
yusuke_robocup 1:d6c8be12a3de 46 180,
yusuke_robocup 1:d6c8be12a3de 47 157,
yusuke_robocup 0:0c7c6eefafe4 48 135,
yusuke_robocup 1:d6c8be12a3de 49 112
yusuke_robocup 0:0c7c6eefafe4 50 };
yusuke_robocup 0:0c7c6eefafe4 51
yusuke_robocup 0:0c7c6eefafe4 52 int moving_ave(int data,int active_ir)
yusuke_robocup 0:0c7c6eefafe4 53 {
yusuke_robocup 2:22b099fbdd14 54 static int tmp[12][ALL_IR]= {{0,0}};
yusuke_robocup 0:0c7c6eefafe4 55 static int sum[ALL_IR] = {0};
yusuke_robocup 0:0c7c6eefafe4 56 int count = 0;
yusuke_robocup 0:0c7c6eefafe4 57
yusuke_robocup 2:22b099fbdd14 58 sum[active_ir] -= tmp[11][active_ir];
yusuke_robocup 0:0c7c6eefafe4 59 sum[active_ir] += data;
yusuke_robocup 2:22b099fbdd14 60 //tmp[13][active_ir] = tmp[12][active_ir];
yusuke_robocup 2:22b099fbdd14 61 //tmp[12][active_ir] = tmp[11][active_ir];
yusuke_robocup 0:0c7c6eefafe4 62 tmp[11][active_ir] = tmp[10][active_ir];
yusuke_robocup 0:0c7c6eefafe4 63 tmp[10][active_ir] = tmp[9][active_ir];
yusuke_robocup 0:0c7c6eefafe4 64 tmp[9][active_ir] = tmp[8][active_ir];
yusuke_robocup 0:0c7c6eefafe4 65 tmp[8][active_ir] = tmp[7][active_ir];
yusuke_robocup 0:0c7c6eefafe4 66 tmp[7][active_ir] = tmp[6][active_ir];
yusuke_robocup 0:0c7c6eefafe4 67 tmp[6][active_ir] = tmp[5][active_ir];
yusuke_robocup 0:0c7c6eefafe4 68 tmp[5][active_ir] = tmp[4][active_ir];
yusuke_robocup 0:0c7c6eefafe4 69 tmp[4][active_ir] = tmp[3][active_ir];
yusuke_robocup 0:0c7c6eefafe4 70 tmp[3][active_ir] = tmp[2][active_ir];
yusuke_robocup 0:0c7c6eefafe4 71 tmp[2][active_ir] = tmp[1][active_ir];
yusuke_robocup 0:0c7c6eefafe4 72 tmp[1][active_ir] = tmp[0][active_ir];
yusuke_robocup 0:0c7c6eefafe4 73 tmp[0][active_ir] = data;
yusuke_robocup 0:0c7c6eefafe4 74
yusuke_robocup 2:22b099fbdd14 75 for(int i=0;i<12;i++){
yusuke_robocup 0:0c7c6eefafe4 76 if(tmp[i][active_ir])count++;
yusuke_robocup 0:0c7c6eefafe4 77 }
yusuke_robocup 0:0c7c6eefafe4 78
yusuke_robocup 0:0c7c6eefafe4 79 return sum[active_ir]/count;
yusuke_robocup 0:0c7c6eefafe4 80 }
yusuke_robocup 0:0c7c6eefafe4 81
yusuke_robocup 0:0c7c6eefafe4 82 void IR_Position(int* direction,int* distance){
yusuke_robocup 0:0c7c6eefafe4 83
yusuke_robocup 0:0c7c6eefafe4 84 int ir_value[ALL_IR+100] = {0};
yusuke_robocup 0:0c7c6eefafe4 85
yusuke_robocup 0:0c7c6eefafe4 86 int active_ir = 0; /* 今回更新する赤外線の番号 */
yusuke_robocup 0:0c7c6eefafe4 87 int memory_ir = 0; /*赤外線時間カウンタ*/
yusuke_robocup 0:0c7c6eefafe4 88 int flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 89 int value = 0;
yusuke_robocup 0:0c7c6eefafe4 90
yusuke_robocup 0:0c7c6eefafe4 91 for(int i=0; i<ALL_IR; i++) {
yusuke_robocup 0:0c7c6eefafe4 92 flag_ir = 1;
yusuke_robocup 0:0c7c6eefafe4 93
yusuke_robocup 0:0c7c6eefafe4 94 DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */
yusuke_robocup 0:0c7c6eefafe4 95
yusuke_robocup 0:0c7c6eefafe4 96 timer_ir.start(); /* タイマー起動 */
yusuke_robocup 0:0c7c6eefafe4 97
yusuke_robocup 0:0c7c6eefafe4 98 if(sensor_ir) { /* もし立ち上がっていたら */
yusuke_robocup 0:0c7c6eefafe4 99 while(sensor_ir) { /* 立ち下がるまで待つ */
yusuke_robocup 0:0c7c6eefafe4 100 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
yusuke_robocup 0:0c7c6eefafe4 101 flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 102 break; /* 立ち上がっている時間が指定時間越えたらブレイク */
yusuke_robocup 0:0c7c6eefafe4 103 }
yusuke_robocup 0:0c7c6eefafe4 104 }
yusuke_robocup 0:0c7c6eefafe4 105 }
yusuke_robocup 0:0c7c6eefafe4 106
yusuke_robocup 0:0c7c6eefafe4 107 timer_ir.stop(); /* タイマー停止 */
yusuke_robocup 0:0c7c6eefafe4 108 timer_ir.reset(); /* タイマーリセット */
yusuke_robocup 0:0c7c6eefafe4 109
yusuke_robocup 0:0c7c6eefafe4 110 if(flag_ir) {
yusuke_robocup 0:0c7c6eefafe4 111 timer_ir.start(); /* タイマー起動 */
yusuke_robocup 0:0c7c6eefafe4 112
yusuke_robocup 0:0c7c6eefafe4 113 while(!(sensor_ir)) { /* 立ち上がるまで待つ */
yusuke_robocup 0:0c7c6eefafe4 114 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
yusuke_robocup 0:0c7c6eefafe4 115 flag_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 116 break; /* 立ち上がっている時間が指定時間越えたらブレイク */
yusuke_robocup 0:0c7c6eefafe4 117 }
yusuke_robocup 0:0c7c6eefafe4 118 }
yusuke_robocup 0:0c7c6eefafe4 119 }
yusuke_robocup 0:0c7c6eefafe4 120
yusuke_robocup 0:0c7c6eefafe4 121 /*ボールが指定時間内に見つかっていたら*/
yusuke_robocup 0:0c7c6eefafe4 122 if(flag_ir) {
yusuke_robocup 0:0c7c6eefafe4 123 memory_ir = timer_ir.read_us();
yusuke_robocup 0:0c7c6eefafe4 124
yusuke_robocup 0:0c7c6eefafe4 125 while(1) {
yusuke_robocup 0:0c7c6eefafe4 126 if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break;
yusuke_robocup 0:0c7c6eefafe4 127
yusuke_robocup 0:0c7c6eefafe4 128 if(!(sensor_ir)) {
yusuke_robocup 2:22b099fbdd14 129 //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir );
yusuke_robocup 2:22b099fbdd14 130 value = (timer_ir.read_us()-memory_ir)/10;
yusuke_robocup 0:0c7c6eefafe4 131
yusuke_robocup 0:0c7c6eefafe4 132 break;
yusuke_robocup 0:0c7c6eefafe4 133 }
yusuke_robocup 0:0c7c6eefafe4 134 }
yusuke_robocup 0:0c7c6eefafe4 135 } else {
yusuke_robocup 0:0c7c6eefafe4 136 /*ボールが見つかっていない場合*/
yusuke_robocup 0:0c7c6eefafe4 137 value = 0;
yusuke_robocup 0:0c7c6eefafe4 138 }
yusuke_robocup 0:0c7c6eefafe4 139 timer_ir.stop(); /* タイマー停止 */
yusuke_robocup 0:0c7c6eefafe4 140 timer_ir.reset(); /* タイマーリセット */
yusuke_robocup 0:0c7c6eefafe4 141
yusuke_robocup 0:0c7c6eefafe4 142 memory_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 143
yusuke_robocup 0:0c7c6eefafe4 144 ir_value[active_ir] = value; //direction array
yusuke_robocup 0:0c7c6eefafe4 145
yusuke_robocup 0:0c7c6eefafe4 146 active_ir++;
yusuke_robocup 0:0c7c6eefafe4 147
yusuke_robocup 0:0c7c6eefafe4 148 if( active_ir >= ALL_IR) {
yusuke_robocup 0:0c7c6eefafe4 149 active_ir = 0;
yusuke_robocup 0:0c7c6eefafe4 150
yusuke_robocup 0:0c7c6eefafe4 151 /***********direction***********/
yusuke_robocup 0:0c7c6eefafe4 152
yusuke_robocup 0:0c7c6eefafe4 153 int min = 100,youso_min = 100;
yusuke_robocup 0:0c7c6eefafe4 154
yusuke_robocup 2:22b099fbdd14 155 for(int i = 0; i<DIREC_IR; i++) {
yusuke_robocup 0:0c7c6eefafe4 156 if((ir_value[i]<min)&&(ir_value[i])) {
yusuke_robocup 0:0c7c6eefafe4 157 min = ir_value[i];
yusuke_robocup 0:0c7c6eefafe4 158 youso_min = i;
yusuke_robocup 0:0c7c6eefafe4 159 }
yusuke_robocup 0:0c7c6eefafe4 160 }
yusuke_robocup 0:0c7c6eefafe4 161
yusuke_robocup 0:0c7c6eefafe4 162 double hiritu = 0;
yusuke_robocup 0:0c7c6eefafe4 163
yusuke_robocup 0:0c7c6eefafe4 164 int direc = 0;
yusuke_robocup 0:0c7c6eefafe4 165
yusuke_robocup 0:0c7c6eefafe4 166 if(youso_min == 0) {
yusuke_robocup 0:0c7c6eefafe4 167 hiritu = (double)ir_value[7]/(double)ir_value[1];
yusuke_robocup 0:0c7c6eefafe4 168 } else if(youso_min ==7) {
yusuke_robocup 0:0c7c6eefafe4 169 hiritu = (double)ir_value[6]/(double)ir_value[0];
yusuke_robocup 0:0c7c6eefafe4 170 } else {
yusuke_robocup 0:0c7c6eefafe4 171 hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1];
yusuke_robocup 0:0c7c6eefafe4 172 }
yusuke_robocup 1:d6c8be12a3de 173
yusuke_robocup 1:d6c8be12a3de 174 if((hiritu <= 0.85)&&(youso_min != 0)) {
yusuke_robocup 0:0c7c6eefafe4 175 direc = youso_min*2-1;
yusuke_robocup 1:d6c8be12a3de 176 }else if((hiritu <= 0.85)&&(youso_min == 0)){
yusuke_robocup 0:0c7c6eefafe4 177 direc = 15;
yusuke_robocup 1:d6c8be12a3de 178 }else if(hiritu >= 1.15) {
yusuke_robocup 0:0c7c6eefafe4 179 direc = youso_min*2+1;
yusuke_robocup 0:0c7c6eefafe4 180 } else {
yusuke_robocup 0:0c7c6eefafe4 181 direc = youso_min*2;
yusuke_robocup 0:0c7c6eefafe4 182 }
yusuke_robocup 1:d6c8be12a3de 183
yusuke_robocup 2:22b099fbdd14 184 /*if(youso_min == 0){
yusuke_robocup 1:d6c8be12a3de 185 direc = 0;
yusuke_robocup 2:22b099fbdd14 186 }*/
yusuke_robocup 0:0c7c6eefafe4 187
yusuke_robocup 0:0c7c6eefafe4 188 /******* direction end *******/
yusuke_robocup 0:0c7c6eefafe4 189
yusuke_robocup 0:0c7c6eefafe4 190 /******* distance *******/
yusuke_robocup 0:0c7c6eefafe4 191
yusuke_robocup 0:0c7c6eefafe4 192 int dista;
yusuke_robocup 0:0c7c6eefafe4 193
yusuke_robocup 2:22b099fbdd14 194 if((ir_value[youso_min]<=28 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 195 dista = 30;
yusuke_robocup 0:0c7c6eefafe4 196 } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 197 dista = 90;
yusuke_robocup 0:0c7c6eefafe4 198 } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) {
yusuke_robocup 0:0c7c6eefafe4 199 dista = 120;
yusuke_robocup 0:0c7c6eefafe4 200 } else if( ir_value[youso_min]>40 + TERM) {
yusuke_robocup 0:0c7c6eefafe4 201 dista = 180;
yusuke_robocup 0:0c7c6eefafe4 202 } else {
yusuke_robocup 0:0c7c6eefafe4 203 dista = 0;
yusuke_robocup 0:0c7c6eefafe4 204 }
yusuke_robocup 0:0c7c6eefafe4 205
yusuke_robocup 2:22b099fbdd14 206 int count_ir = 0,total_ir = 0;
yusuke_robocup 2:22b099fbdd14 207
yusuke_robocup 2:22b099fbdd14 208 for(int i=0; i<DIREC_IR; i++){
yusuke_robocup 2:22b099fbdd14 209 if(ir_value[i]){
yusuke_robocup 2:22b099fbdd14 210 total_ir += ir_value[i];
yusuke_robocup 2:22b099fbdd14 211 count_ir++;
yusuke_robocup 1:d6c8be12a3de 212 }
yusuke_robocup 2:22b099fbdd14 213 }
yusuke_robocup 2:22b099fbdd14 214
yusuke_robocup 2:22b099fbdd14 215 double hihhihi = 0;
yusuke_robocup 2:22b099fbdd14 216
yusuke_robocup 2:22b099fbdd14 217 hihhihi = (double)ir_value[youso_min]/(double)ir_value[8];
yusuke_robocup 2:22b099fbdd14 218
yusuke_robocup 1:d6c8be12a3de 219
yusuke_robocup 2:22b099fbdd14 220 if((direc == 0)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 221 dista = 10;
yusuke_robocup 2:22b099fbdd14 222 }else if((direc == 1)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 223 dista = 10;
yusuke_robocup 2:22b099fbdd14 224 }else if((direc == 2)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 225 dista = 10;
yusuke_robocup 2:22b099fbdd14 226 }else if((direc == 3)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 227 dista = 10;
yusuke_robocup 2:22b099fbdd14 228 }else if((direc == 4)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 229 dista = 10;
yusuke_robocup 2:22b099fbdd14 230 }else if((direc == 5)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 231 dista = 10;
yusuke_robocup 2:22b099fbdd14 232 }else if((direc == 6)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 233 dista = 10;
yusuke_robocup 2:22b099fbdd14 234 }else if((direc == 7)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 235 dista = 10;
yusuke_robocup 2:22b099fbdd14 236 }else if((direc == 8)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 237 dista = 10;
yusuke_robocup 2:22b099fbdd14 238 }else if((direc == 9)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 239 dista = 10;
yusuke_robocup 2:22b099fbdd14 240 }else if((direc == 10)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 241 dista = 10;
yusuke_robocup 2:22b099fbdd14 242 }else if((direc == 11)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 243 dista = 10;
yusuke_robocup 2:22b099fbdd14 244 }else if((direc == 12)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 245 dista = 10;
yusuke_robocup 2:22b099fbdd14 246 }else if((direc == 13)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 247 dista = 10;
yusuke_robocup 2:22b099fbdd14 248 }else if((direc == 14)&&(hihhihi >= 0.65)){
yusuke_robocup 2:22b099fbdd14 249 dista = 10;
yusuke_robocup 2:22b099fbdd14 250 }else if((direc == 15)&&(hihhihi >= 0.80)){
yusuke_robocup 2:22b099fbdd14 251 dista = 10;
yusuke_robocup 2:22b099fbdd14 252 }
yusuke_robocup 2:22b099fbdd14 253
yusuke_robocup 2:22b099fbdd14 254
yusuke_robocup 0:0c7c6eefafe4 255 /******** distance end *******/
yusuke_robocup 0:0c7c6eefafe4 256
yusuke_robocup 0:0c7c6eefafe4 257 *direction = Convert_Direction[direc];
yusuke_robocup 0:0c7c6eefafe4 258 *distance = dista;
yusuke_robocup 0:0c7c6eefafe4 259 }
yusuke_robocup 0:0c7c6eefafe4 260 }
yusuke_robocup 0:0c7c6eefafe4 261 }
yusuke_robocup 0:0c7c6eefafe4 262
yusuke_robocup 0:0c7c6eefafe4 263 int main ()
yusuke_robocup 0:0c7c6eefafe4 264 {
yusuke_robocup 0:0c7c6eefafe4 265 pc.baud(9600);
yusuke_robocup 0:0c7c6eefafe4 266
yusuke_robocup 0:0c7c6eefafe4 267 int derection = 0;
yusuke_robocup 0:0c7c6eefafe4 268 int distance = 0;
yusuke_robocup 0:0c7c6eefafe4 269
yusuke_robocup 0:0c7c6eefafe4 270 for(;;){
yusuke_robocup 0:0c7c6eefafe4 271 IR_Position(&derection,&distance);
yusuke_robocup 0:0c7c6eefafe4 272
yusuke_robocup 0:0c7c6eefafe4 273 pc.printf("derection:%d distance:%d\n",derection,distance);
yusuke_robocup 0:0c7c6eefafe4 274 }
yusuke_robocup 0:0c7c6eefafe4 275 }