ryo seki / Mbed 2 deprecated ver1_2_0

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?

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