ryo seki
/
ver1_2_2
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 00006 extern Timer timer_ir; 00007 extern Serial pc; // tx, rx 00008 00009 int direction = 0; 00010 int Distance = 0; 00011 00012 int IR_found; 00013 00014 00015 void IR_Position(){ 00016 00017 int ir_value[ALL_IR+100] = {0}; 00018 00019 int active_ir = 0; /* 今回更新する赤外線の番号 */ 00020 int memory_ir = 0; /*赤外線時間カウンタ*/ 00021 int flag_ir = 0; 00022 int value = 0; 00023 00024 for(int i=0; i<ALL_IR; i++) { 00025 flag_ir = 1; 00026 00027 DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */ 00028 00029 timer_ir.start(); /* タイマー起動 */ 00030 00031 if(sensor_ir) { /* もし立ち上がっていたら */ 00032 while(sensor_ir) { /* 立ち下がるまで待つ */ 00033 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { 00034 flag_ir = 0; 00035 break; /* 立ち上がっている時間が指定時間越えたらブレイク */ 00036 } 00037 } 00038 } 00039 00040 timer_ir.stop(); /* タイマー停止 */ 00041 timer_ir.reset(); /* タイマーリセット */ 00042 00043 if(flag_ir) { 00044 timer_ir.start(); /* タイマー起動 */ 00045 00046 while(!(sensor_ir)) { /* 立ち上がるまで待つ */ 00047 if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { 00048 flag_ir = 0; 00049 break; /* 立ち上がっている時間が指定時間越えたらブレイク */ 00050 } 00051 } 00052 } 00053 00054 /*ボールが指定時間内に見つかっていたら*/ 00055 if(flag_ir) { 00056 memory_ir = timer_ir.read_us(); 00057 00058 while(1) { 00059 if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break; 00060 00061 if(!(sensor_ir)) { 00062 //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir ); 00063 value = (timer_ir.read_us()-memory_ir)/10; 00064 00065 break; 00066 } 00067 } 00068 } else { 00069 /*ボールが見つかっていない場合*/ 00070 value = 0; 00071 } 00072 timer_ir.stop(); /* タイマー停止 */ 00073 timer_ir.reset(); /* タイマーリセット */ 00074 00075 memory_ir = 0; 00076 00077 ir_value[active_ir] = value; //direction array 00078 00079 active_ir++; 00080 00081 if( active_ir >= ALL_IR) { 00082 active_ir = 0; 00083 00084 /***********direction***********/ 00085 00086 int min = 100,youso_min = 100; 00087 00088 for(int i = 0; i<DIREC_IR; i++) { 00089 if((ir_value[i]<min)&&(ir_value[i])) { 00090 min = ir_value[i]; 00091 youso_min = i; 00092 } 00093 } 00094 00095 double hiritu = 0; 00096 00097 int direc = 0; 00098 00099 if(youso_min == 0) { 00100 hiritu = (double)ir_value[7]/(double)ir_value[1]; 00101 } else if(youso_min ==7) { 00102 hiritu = (double)ir_value[6]/(double)ir_value[0]; 00103 } else { 00104 hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1]; 00105 } 00106 00107 if((hiritu <= 0.85)&&(youso_min != 0)) { 00108 direc = youso_min*2-1; 00109 }else if((hiritu <= 0.85)&&(youso_min == 0)){ 00110 direc = 15; 00111 }else if(hiritu >= 1.15) { 00112 direc = youso_min*2+1; 00113 } else { 00114 direc = youso_min*2; 00115 } 00116 00117 /*if(youso_min == 0){ 00118 direc = 0; 00119 }*/ 00120 00121 /******* direction end *******/ 00122 00123 /******* distance *******/ 00124 00125 int dista; 00126 00127 if((ir_value[youso_min]<=28 + TERM)) { 00128 dista = 30; 00129 } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) { 00130 dista = 90; 00131 } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) { 00132 dista = 120; 00133 } else if( ir_value[youso_min]>40 + TERM) { 00134 dista = 180; 00135 } else { 00136 dista = 0; 00137 } 00138 00139 int count_ir = 0,total_ir = 0; 00140 00141 for(int i=0; i<DIREC_IR; i++){ 00142 if(ir_value[i]){ 00143 total_ir += ir_value[i]; 00144 count_ir++; 00145 } 00146 } 00147 00148 double hihhihi = 0; 00149 00150 hihhihi = (double)ir_value[youso_min]/(double)ir_value[8]; 00151 00152 00153 if((direc == 0)&&(hihhihi >= 0.80)){ 00154 dista = 10; 00155 }else if((direc == 1)&&(hihhihi >= 0.80)){ 00156 dista = 10; 00157 }else if((direc == 2)&&(hihhihi >= 0.65)){ 00158 dista = 10; 00159 }else if((direc == 3)&&(hihhihi >= 0.65)){ 00160 dista = 10; 00161 }else if((direc == 4)&&(hihhihi >= 0.80)){ 00162 dista = 10; 00163 }else if((direc == 5)&&(hihhihi >= 0.65)){ 00164 dista = 10; 00165 }else if((direc == 6)&&(hihhihi >= 0.65)){ 00166 dista = 10; 00167 }else if((direc == 7)&&(hihhihi >= 0.80)){ 00168 dista = 10; 00169 }else if((direc == 8)&&(hihhihi >= 0.80)){ 00170 dista = 10; 00171 }else if((direc == 9)&&(hihhihi >= 0.80)){ 00172 dista = 10; 00173 }else if((direc == 10)&&(hihhihi >= 0.65)){ 00174 dista = 10; 00175 }else if((direc == 11)&&(hihhihi >= 0.65)){ 00176 dista = 10; 00177 }else if((direc == 12)&&(hihhihi >= 0.80)){ 00178 dista = 10; 00179 }else if((direc == 13)&&(hihhihi >= 0.65)){ 00180 dista = 10; 00181 }else if((direc == 14)&&(hihhihi >= 0.65)){ 00182 dista = 10; 00183 }else if((direc == 15)&&(hihhihi >= 0.80)){ 00184 dista = 10; 00185 } 00186 00187 int count = 0; 00188 00189 for(int i=0;i<DIREC_IR;i++){ 00190 if(ir_value[i])count++; 00191 } 00192 00193 if(count) IR_found = 1; 00194 else IR_found = 0; 00195 00196 00197 /******** distance end *******/ 00198 00199 direction = direc; 00200 Distance = dista; 00201 00202 //printf("derection:%d distance:%d\n",direction,Distance); 00203 } 00204 } 00205 }
Generated on Thu Jul 14 2022 06:03:18 by
1.7.2