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