robocup
Dependencies: HMC6352 PID mbed
IR.cpp@0:13ab960fc61f, 2013-03-08 (annotated)
- Committer:
- akudohune
- Date:
- Fri Mar 08 07:13:29 2013 +0000
- Revision:
- 0:13ab960fc61f
ver1_2_0;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akudohune | 0:13ab960fc61f | 1 | |
akudohune | 0:13ab960fc61f | 2 | #include "mbed.h" |
akudohune | 0:13ab960fc61f | 3 | #include "IR.h" |
akudohune | 0:13ab960fc61f | 4 | |
akudohune | 0:13ab960fc61f | 5 | |
akudohune | 0:13ab960fc61f | 6 | extern Timer timer_ir; |
akudohune | 0:13ab960fc61f | 7 | extern Serial pc; // tx, rx |
akudohune | 0:13ab960fc61f | 8 | |
akudohune | 0:13ab960fc61f | 9 | int direction = 0; |
akudohune | 0:13ab960fc61f | 10 | int Distance = 0; |
akudohune | 0:13ab960fc61f | 11 | |
akudohune | 0:13ab960fc61f | 12 | int IR_found; |
akudohune | 0:13ab960fc61f | 13 | |
akudohune | 0:13ab960fc61f | 14 | |
akudohune | 0:13ab960fc61f | 15 | void IR_Position(){ |
akudohune | 0:13ab960fc61f | 16 | |
akudohune | 0:13ab960fc61f | 17 | int ir_value[ALL_IR+100] = {0}; |
akudohune | 0:13ab960fc61f | 18 | |
akudohune | 0:13ab960fc61f | 19 | int active_ir = 0; /* 今回更新する赤外線の番号 */ |
akudohune | 0:13ab960fc61f | 20 | int memory_ir = 0; /*赤外線時間カウンタ*/ |
akudohune | 0:13ab960fc61f | 21 | int flag_ir = 0; |
akudohune | 0:13ab960fc61f | 22 | int value = 0; |
akudohune | 0:13ab960fc61f | 23 | |
akudohune | 0:13ab960fc61f | 24 | for(int i=0; i<ALL_IR; i++) { |
akudohune | 0:13ab960fc61f | 25 | flag_ir = 1; |
akudohune | 0:13ab960fc61f | 26 | |
akudohune | 0:13ab960fc61f | 27 | DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */ |
akudohune | 0:13ab960fc61f | 28 | |
akudohune | 0:13ab960fc61f | 29 | timer_ir.start(); /* タイマー起動 */ |
akudohune | 0:13ab960fc61f | 30 | |
akudohune | 0:13ab960fc61f | 31 | if(sensor_ir) { /* もし立ち上がっていたら */ |
akudohune | 0:13ab960fc61f | 32 | while(sensor_ir) { /* 立ち下がるまで待つ */ |
akudohune | 0:13ab960fc61f | 33 | if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { |
akudohune | 0:13ab960fc61f | 34 | flag_ir = 0; |
akudohune | 0:13ab960fc61f | 35 | break; /* 立ち上がっている時間が指定時間越えたらブレイク */ |
akudohune | 0:13ab960fc61f | 36 | } |
akudohune | 0:13ab960fc61f | 37 | } |
akudohune | 0:13ab960fc61f | 38 | } |
akudohune | 0:13ab960fc61f | 39 | |
akudohune | 0:13ab960fc61f | 40 | timer_ir.stop(); /* タイマー停止 */ |
akudohune | 0:13ab960fc61f | 41 | timer_ir.reset(); /* タイマーリセット */ |
akudohune | 0:13ab960fc61f | 42 | |
akudohune | 0:13ab960fc61f | 43 | if(flag_ir) { |
akudohune | 0:13ab960fc61f | 44 | timer_ir.start(); /* タイマー起動 */ |
akudohune | 0:13ab960fc61f | 45 | |
akudohune | 0:13ab960fc61f | 46 | while(!(sensor_ir)) { /* 立ち上がるまで待つ */ |
akudohune | 0:13ab960fc61f | 47 | if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { |
akudohune | 0:13ab960fc61f | 48 | flag_ir = 0; |
akudohune | 0:13ab960fc61f | 49 | break; /* 立ち上がっている時間が指定時間越えたらブレイク */ |
akudohune | 0:13ab960fc61f | 50 | } |
akudohune | 0:13ab960fc61f | 51 | } |
akudohune | 0:13ab960fc61f | 52 | } |
akudohune | 0:13ab960fc61f | 53 | |
akudohune | 0:13ab960fc61f | 54 | /*ボールが指定時間内に見つかっていたら*/ |
akudohune | 0:13ab960fc61f | 55 | if(flag_ir) { |
akudohune | 0:13ab960fc61f | 56 | memory_ir = timer_ir.read_us(); |
akudohune | 0:13ab960fc61f | 57 | |
akudohune | 0:13ab960fc61f | 58 | while(1) { |
akudohune | 0:13ab960fc61f | 59 | if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break; |
akudohune | 0:13ab960fc61f | 60 | |
akudohune | 0:13ab960fc61f | 61 | if(!(sensor_ir)) { |
akudohune | 0:13ab960fc61f | 62 | //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir ); |
akudohune | 0:13ab960fc61f | 63 | value = (timer_ir.read_us()-memory_ir)/10; |
akudohune | 0:13ab960fc61f | 64 | |
akudohune | 0:13ab960fc61f | 65 | break; |
akudohune | 0:13ab960fc61f | 66 | } |
akudohune | 0:13ab960fc61f | 67 | } |
akudohune | 0:13ab960fc61f | 68 | } else { |
akudohune | 0:13ab960fc61f | 69 | /*ボールが見つかっていない場合*/ |
akudohune | 0:13ab960fc61f | 70 | value = 0; |
akudohune | 0:13ab960fc61f | 71 | } |
akudohune | 0:13ab960fc61f | 72 | timer_ir.stop(); /* タイマー停止 */ |
akudohune | 0:13ab960fc61f | 73 | timer_ir.reset(); /* タイマーリセット */ |
akudohune | 0:13ab960fc61f | 74 | |
akudohune | 0:13ab960fc61f | 75 | memory_ir = 0; |
akudohune | 0:13ab960fc61f | 76 | |
akudohune | 0:13ab960fc61f | 77 | ir_value[active_ir] = value; //direction array |
akudohune | 0:13ab960fc61f | 78 | |
akudohune | 0:13ab960fc61f | 79 | active_ir++; |
akudohune | 0:13ab960fc61f | 80 | |
akudohune | 0:13ab960fc61f | 81 | if( active_ir >= ALL_IR) { |
akudohune | 0:13ab960fc61f | 82 | active_ir = 0; |
akudohune | 0:13ab960fc61f | 83 | |
akudohune | 0:13ab960fc61f | 84 | /***********direction***********/ |
akudohune | 0:13ab960fc61f | 85 | |
akudohune | 0:13ab960fc61f | 86 | int min = 100,youso_min = 100; |
akudohune | 0:13ab960fc61f | 87 | |
akudohune | 0:13ab960fc61f | 88 | for(int i = 0; i<DIREC_IR; i++) { |
akudohune | 0:13ab960fc61f | 89 | if((ir_value[i]<min)&&(ir_value[i])) { |
akudohune | 0:13ab960fc61f | 90 | min = ir_value[i]; |
akudohune | 0:13ab960fc61f | 91 | youso_min = i; |
akudohune | 0:13ab960fc61f | 92 | } |
akudohune | 0:13ab960fc61f | 93 | } |
akudohune | 0:13ab960fc61f | 94 | |
akudohune | 0:13ab960fc61f | 95 | double hiritu = 0; |
akudohune | 0:13ab960fc61f | 96 | |
akudohune | 0:13ab960fc61f | 97 | int direc = 0; |
akudohune | 0:13ab960fc61f | 98 | |
akudohune | 0:13ab960fc61f | 99 | if(youso_min == 0) { |
akudohune | 0:13ab960fc61f | 100 | hiritu = (double)ir_value[7]/(double)ir_value[1]; |
akudohune | 0:13ab960fc61f | 101 | } else if(youso_min ==7) { |
akudohune | 0:13ab960fc61f | 102 | hiritu = (double)ir_value[6]/(double)ir_value[0]; |
akudohune | 0:13ab960fc61f | 103 | } else { |
akudohune | 0:13ab960fc61f | 104 | hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1]; |
akudohune | 0:13ab960fc61f | 105 | } |
akudohune | 0:13ab960fc61f | 106 | |
akudohune | 0:13ab960fc61f | 107 | if((hiritu <= 0.85)&&(youso_min != 0)) { |
akudohune | 0:13ab960fc61f | 108 | direc = youso_min*2-1; |
akudohune | 0:13ab960fc61f | 109 | }else if((hiritu <= 0.85)&&(youso_min == 0)){ |
akudohune | 0:13ab960fc61f | 110 | direc = 15; |
akudohune | 0:13ab960fc61f | 111 | }else if(hiritu >= 1.15) { |
akudohune | 0:13ab960fc61f | 112 | direc = youso_min*2+1; |
akudohune | 0:13ab960fc61f | 113 | } else { |
akudohune | 0:13ab960fc61f | 114 | direc = youso_min*2; |
akudohune | 0:13ab960fc61f | 115 | } |
akudohune | 0:13ab960fc61f | 116 | |
akudohune | 0:13ab960fc61f | 117 | /*if(youso_min == 0){ |
akudohune | 0:13ab960fc61f | 118 | direc = 0; |
akudohune | 0:13ab960fc61f | 119 | }*/ |
akudohune | 0:13ab960fc61f | 120 | |
akudohune | 0:13ab960fc61f | 121 | /******* direction end *******/ |
akudohune | 0:13ab960fc61f | 122 | |
akudohune | 0:13ab960fc61f | 123 | /******* distance *******/ |
akudohune | 0:13ab960fc61f | 124 | |
akudohune | 0:13ab960fc61f | 125 | int dista; |
akudohune | 0:13ab960fc61f | 126 | |
akudohune | 0:13ab960fc61f | 127 | if((ir_value[youso_min]<=28 + TERM)) { |
akudohune | 0:13ab960fc61f | 128 | dista = 30; |
akudohune | 0:13ab960fc61f | 129 | } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) { |
akudohune | 0:13ab960fc61f | 130 | dista = 90; |
akudohune | 0:13ab960fc61f | 131 | } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) { |
akudohune | 0:13ab960fc61f | 132 | dista = 120; |
akudohune | 0:13ab960fc61f | 133 | } else if( ir_value[youso_min]>40 + TERM) { |
akudohune | 0:13ab960fc61f | 134 | dista = 180; |
akudohune | 0:13ab960fc61f | 135 | } else { |
akudohune | 0:13ab960fc61f | 136 | dista = 0; |
akudohune | 0:13ab960fc61f | 137 | } |
akudohune | 0:13ab960fc61f | 138 | |
akudohune | 0:13ab960fc61f | 139 | int count_ir = 0,total_ir = 0; |
akudohune | 0:13ab960fc61f | 140 | |
akudohune | 0:13ab960fc61f | 141 | for(int i=0; i<DIREC_IR; i++){ |
akudohune | 0:13ab960fc61f | 142 | if(ir_value[i]){ |
akudohune | 0:13ab960fc61f | 143 | total_ir += ir_value[i]; |
akudohune | 0:13ab960fc61f | 144 | count_ir++; |
akudohune | 0:13ab960fc61f | 145 | } |
akudohune | 0:13ab960fc61f | 146 | } |
akudohune | 0:13ab960fc61f | 147 | |
akudohune | 0:13ab960fc61f | 148 | double hihhihi = 0; |
akudohune | 0:13ab960fc61f | 149 | |
akudohune | 0:13ab960fc61f | 150 | hihhihi = (double)ir_value[youso_min]/(double)ir_value[8]; |
akudohune | 0:13ab960fc61f | 151 | |
akudohune | 0:13ab960fc61f | 152 | |
akudohune | 0:13ab960fc61f | 153 | if((direc == 0)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 154 | dista = 10; |
akudohune | 0:13ab960fc61f | 155 | }else if((direc == 1)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 156 | dista = 10; |
akudohune | 0:13ab960fc61f | 157 | }else if((direc == 2)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 158 | dista = 10; |
akudohune | 0:13ab960fc61f | 159 | }else if((direc == 3)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 160 | dista = 10; |
akudohune | 0:13ab960fc61f | 161 | }else if((direc == 4)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 162 | dista = 10; |
akudohune | 0:13ab960fc61f | 163 | }else if((direc == 5)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 164 | dista = 10; |
akudohune | 0:13ab960fc61f | 165 | }else if((direc == 6)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 166 | dista = 10; |
akudohune | 0:13ab960fc61f | 167 | }else if((direc == 7)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 168 | dista = 10; |
akudohune | 0:13ab960fc61f | 169 | }else if((direc == 8)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 170 | dista = 10; |
akudohune | 0:13ab960fc61f | 171 | }else if((direc == 9)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 172 | dista = 10; |
akudohune | 0:13ab960fc61f | 173 | }else if((direc == 10)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 174 | dista = 10; |
akudohune | 0:13ab960fc61f | 175 | }else if((direc == 11)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 176 | dista = 10; |
akudohune | 0:13ab960fc61f | 177 | }else if((direc == 12)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 178 | dista = 10; |
akudohune | 0:13ab960fc61f | 179 | }else if((direc == 13)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 180 | dista = 10; |
akudohune | 0:13ab960fc61f | 181 | }else if((direc == 14)&&(hihhihi >= 0.65)){ |
akudohune | 0:13ab960fc61f | 182 | dista = 10; |
akudohune | 0:13ab960fc61f | 183 | }else if((direc == 15)&&(hihhihi >= 0.80)){ |
akudohune | 0:13ab960fc61f | 184 | dista = 10; |
akudohune | 0:13ab960fc61f | 185 | } |
akudohune | 0:13ab960fc61f | 186 | |
akudohune | 0:13ab960fc61f | 187 | int count = 0; |
akudohune | 0:13ab960fc61f | 188 | |
akudohune | 0:13ab960fc61f | 189 | for(int i=0;i<DIREC_IR;i++){ |
akudohune | 0:13ab960fc61f | 190 | if(ir_value[i])count++; |
akudohune | 0:13ab960fc61f | 191 | } |
akudohune | 0:13ab960fc61f | 192 | |
akudohune | 0:13ab960fc61f | 193 | if(count) IR_found = 1; |
akudohune | 0:13ab960fc61f | 194 | else IR_found = 0; |
akudohune | 0:13ab960fc61f | 195 | |
akudohune | 0:13ab960fc61f | 196 | |
akudohune | 0:13ab960fc61f | 197 | /******** distance end *******/ |
akudohune | 0:13ab960fc61f | 198 | |
akudohune | 0:13ab960fc61f | 199 | direction = direc; |
akudohune | 0:13ab960fc61f | 200 | Distance = dista; |
akudohune | 0:13ab960fc61f | 201 | |
akudohune | 0:13ab960fc61f | 202 | //printf("derection:%d distance:%d\n",direction,Distance); |
akudohune | 0:13ab960fc61f | 203 | } |
akudohune | 0:13ab960fc61f | 204 | } |
akudohune | 0:13ab960fc61f | 205 | } |