yusuke takahashi / Mbed 2 deprecated ver1_2_2_1

sasasa

Dependencies:   HMC6352 PID eeprom mbed

Fork of ver1_2_2_1 by ryo seki

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?

UserRevisionLine numberNew 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 }