ryo seki / Mbed 2 deprecated serialSend5

a

Dependencies:   TextLCD mbed

IR.cpp@1:ed00e9c4ba6e, 2013-04-30 (annotated)

Committer:
akudohune
Date:
Tue Apr 30 09:06:57 2013 +0000
Revision:
1:ed00e9c4ba6e
Parent:
0:62ac378df546 
ooo

Who changed what in which revision?

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