ryo seki
/
serialSend5_5
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
IR.cpp
00001 00002 #include "mbed.h" 00003 #include "IR.h" 00004 00005 extern Timer timer_ir; 00006 00007 int direction = 0; 00008 int Distance = 0; 00009 int IR_found = 0; 00010 00011 void IR_Position(){ 00012 00013 int ir_value[ALL_IR+100] = {0}; 00014 00015 int active_ir = 0; /* 今回更新する赤外線の番号 */ 00016 int memory_ir = 0; /*赤外線時間カウンタ*/ 00017 int flag_ir = 0; 00018 int value = 0; 00019 00020 static int direc = 99; 00021 static int past_direc = 99; 00022 00023 int liftball; 00024 00025 for(int i=0; i<ALL_IR; i++) { 00026 flag_ir = 1; 00027 00028 DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */ 00029 00030 timer_ir.start(); /* タイマー起動 */ 00031 00032 if(sensor_ir) { /* もし立ち上がっていたら */ 00033 while(sensor_ir) { /* 立ち下がるまで待つ */ 00034 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { 00035 flag_ir = 0; 00036 break; /* 立ち上がっている時間が指定時間越えたらブレイク */ 00037 } 00038 } 00039 } 00040 00041 timer_ir.stop(); /* タイマー停止 */ 00042 timer_ir.reset(); /* タイマーリセット */ 00043 00044 if(flag_ir) { 00045 timer_ir.start(); /* タイマー起動 */ 00046 00047 while(!(sensor_ir)) { /* 立ち上がるまで待つ */ 00048 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { 00049 flag_ir = 0; 00050 break; /* 立ち上がっている時間が指定時間越えたらブレイク */ 00051 } 00052 } 00053 } 00054 00055 /*ボールが指定時間内に見つかっていたら*/ 00056 if(flag_ir) { 00057 memory_ir = timer_ir.read_us(); 00058 00059 while(1) { 00060 if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break; 00061 00062 if(!(sensor_ir)) { 00063 //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir ); 00064 value = (timer_ir.read_us()-memory_ir)/10; 00065 00066 break; 00067 } 00068 } 00069 } else { 00070 /*ボールが見つかっていない場合*/ 00071 value = 0; 00072 } 00073 timer_ir.stop(); /* タイマー停止 */ 00074 timer_ir.reset(); /* タイマーリセット */ 00075 00076 memory_ir = 0; 00077 00078 ir_value[active_ir] = value; //direction array 00079 00080 if((ir_value[0])&&(active_ir == 0)){ 00081 ir_value[0] -= 3; 00082 } 00083 00084 if((ir_value[8])&&(active_ir == 8)){ 00085 ir_value[8] -= 2; 00086 } 00087 00088 active_ir++; 00089 00090 00091 if( active_ir >= ALL_IR) { 00092 active_ir = 0; 00093 00094 /***********direction***********/ 00095 00096 int min = 100,youso_min = 100; 00097 00098 for(int i = 0; i<DIREC_IR; i++) { 00099 if((ir_value[i]<min)&&(ir_value[i])) { 00100 min = ir_value[i]; 00101 youso_min = i; 00102 } 00103 } 00104 00105 if(youso_min == 8){ 00106 direc = 1; 00107 }else if(youso_min == 9){ 00108 direc = 15; 00109 }else{ 00110 direc = youso_min * 2; 00111 } 00112 /* 00113 if(ir_value[youso_min] > 50){ 00114 liftball = 1; 00115 }else{ 00116 liftball = 0; 00117 } 00118 00119 if((past_direc != 99)&&(liftball == 0)){ 00120 if(past_direc == 0){ 00121 if((direc >= 4)&&(direc <= 12)){ 00122 direc = past_direc; 00123 } 00124 }else if(past_direc == 1){ 00125 if((direc >= 4)&&(direc <= 14)){ 00126 direc = past_direc; 00127 } 00128 }else if(past_direc == 15){ 00129 if((direc >= 2)&&(direc <= 12)){ 00130 direc = past_direc; 00131 } 00132 }else{ 00133 if(abs(past_direc - direc) >= 4){ 00134 direc = past_direc; 00135 } 00136 } 00137 }*/ 00138 00139 //direc = youso_min * 2; 00140 00141 /******* direction end *******/ 00142 00143 /******* distance *******/ 00144 00145 int dista; 00146 00147 if((ir_value[youso_min]<=28 + TERM)) { 00148 dista = 30; 00149 } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) { 00150 dista = 90; 00151 } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) { 00152 dista = 120; 00153 } else if( ir_value[youso_min]>40 + TERM) { 00154 dista = 180; 00155 } else { 00156 dista = 0; 00157 } 00158 00159 int count_ir = 0,total_ir = 0; 00160 00161 for(int i=0; i<DIREC_IR; i++){ 00162 if(ir_value[i]){ 00163 total_ir += ir_value[i]; 00164 count_ir++; 00165 } 00166 } 00167 00168 double hihhihi = 0; 00169 00170 if(!ir_value[10]) ir_value[10] = 1000000; 00171 00172 hihhihi = (double)ir_value[youso_min]/(double)ir_value[10]; 00173 00174 if(ir_value[10] <= 38){ 00175 if((direc == 0)&&(hihhihi >= 1.0)){ 00176 dista = 10; 00177 }else if((direc == 1)&&(hihhihi >= 1.0)){ 00178 dista = 10; 00179 }else if((direc == 2)&&(hihhihi >= 0.75)){ 00180 dista = 10; 00181 }else if((direc == 3)&&(hihhihi >= 0.65)){ 00182 dista = 10; 00183 }else if((direc == 4)&&(hihhihi >= 0.60)){ 00184 dista = 10; 00185 }else if((direc == 5)&&(hihhihi >= 0.60)){ 00186 dista = 10; 00187 }else if((direc == 6)&&(hihhihi >= 0.70)){ 00188 dista = 10; 00189 }else if((direc == 7)&&(hihhihi >= 0.70)){ 00190 dista = 10; 00191 }else if((direc == 8)&&(hihhihi >= 0.70)){ 00192 dista = 10; 00193 }else if((direc == 9)&&(hihhihi >= 0.70)){ 00194 dista = 10; 00195 }else if((direc == 10)&&(hihhihi >= 0.60)){ 00196 dista = 10; 00197 }else if((direc == 11)&&(hihhihi >= 0.60)){ 00198 dista = 10; 00199 }else if((direc == 12)&&(hihhihi >= 0.60)){ 00200 dista = 10; 00201 }else if((direc == 13)&&(hihhihi >= 0.65)){ 00202 dista = 10; 00203 }else if((direc == 14)&&(hihhihi >= 0.75)){ 00204 dista = 10; 00205 }else if((direc == 15)&&(hihhihi >= 1.0)){ 00206 dista = 10; 00207 } 00208 } 00209 00210 int count = 0; 00211 00212 for(int i=0;i<DIREC_IR;i++){ 00213 if(ir_value[i])count++; 00214 } 00215 00216 if(count) IR_found = 1; 00217 else IR_found = 0; 00218 00219 00220 /******** distance end *******/ 00221 00222 past_direc = direc; 00223 00224 direction = direc; 00225 Distance = dista; 00226 00227 //printf("derection:%d distance:%d\n",direction,Distance); 00228 } 00229 } 00230 }
Generated on Wed Jul 20 2022 05:51:04 by
1.7.2