ryo seki / Mbed 2 deprecated ver1_2_2_1

test

Dependencies:   HMC6352 PID mbed

Fork of ver1_2_2 by ryo seki

IR.cpp@1:89408fff7cc9, 2013-03-10 (annotated)

Committer:
akudohune
Date:
Sun Mar 10 07:31:31 2013 +0000
Revision:
1:89408fff7cc9
Parent:
0:74bf4953c0d1 
new_cup;

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