ryo seki
/
serialSend5
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 active_ir++; 00081 00082 if( active_ir >= ALL_IR) { 00083 active_ir = 0; 00084 00085 /***********direction***********/ 00086 00087 int min = 100,youso_min = 100; 00088 00089 for(int i = 0; i<DIREC_IR; i++) { 00090 if((ir_value[i]<min)&&(ir_value[i])) { 00091 min = ir_value[i]; 00092 youso_min = i; 00093 } 00094 } 00095 00096 if(youso_min == 8){ 00097 direc = 1; 00098 }else if(youso_min == 9){ 00099 direc = 15; 00100 }else{ 00101 direc = youso_min * 2; 00102 } 00103 /* 00104 if(ir_value[youso_min] > 50){ 00105 liftball = 1; 00106 }else{ 00107 liftball = 0; 00108 } 00109 00110 if((past_direc != 99)&&(liftball == 0)){ 00111 if(past_direc == 0){ 00112 if((direc >= 4)&&(direc <= 12)){ 00113 direc = past_direc; 00114 } 00115 }else if(past_direc == 1){ 00116 if((direc >= 4)&&(direc <= 14)){ 00117 direc = past_direc; 00118 } 00119 }else if(past_direc == 15){ 00120 if((direc >= 2)&&(direc <= 12)){ 00121 direc = past_direc; 00122 } 00123 }else{ 00124 if(abs(past_direc - direc) >= 4){ 00125 direc = past_direc; 00126 } 00127 } 00128 }*/ 00129 00130 //direc = youso_min * 2; 00131 00132 /******* direction end *******/ 00133 00134 /******* distance *******/ 00135 00136 int dista; 00137 00138 if((ir_value[youso_min]<=28 + TERM)) { 00139 dista = 30; 00140 } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) { 00141 dista = 90; 00142 } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) { 00143 dista = 120; 00144 } else if( ir_value[youso_min]>40 + TERM) { 00145 dista = 180; 00146 } else { 00147 dista = 0; 00148 } 00149 00150 int count_ir = 0,total_ir = 0; 00151 00152 for(int i=0; i<DIREC_IR; i++){ 00153 if(ir_value[i]){ 00154 total_ir += ir_value[i]; 00155 count_ir++; 00156 } 00157 } 00158 00159 double hihhihi = 0; 00160 00161 if(!ir_value[10]) ir_value[10] = 1000000; 00162 00163 hihhihi = (double)ir_value[youso_min]/(double)ir_value[10]; 00164 00165 if(ir_value[10] <= 38){ 00166 if((direc == 0)&&(hihhihi >= 1.0)){ 00167 dista = 10; 00168 }else if((direc == 1)&&(hihhihi >= 1.0)){ 00169 dista = 10; 00170 }else if((direc == 2)&&(hihhihi >= 0.75)){ 00171 dista = 10; 00172 }else if((direc == 3)&&(hihhihi >= 0.65)){ 00173 dista = 10; 00174 }else if((direc == 4)&&(hihhihi >= 0.60)){ 00175 dista = 10; 00176 }else if((direc == 5)&&(hihhihi >= 0.60)){ 00177 dista = 10; 00178 }else if((direc == 6)&&(hihhihi >= 0.70)){ 00179 dista = 10; 00180 }else if((direc == 7)&&(hihhihi >= 0.70)){ 00181 dista = 10; 00182 }else if((direc == 8)&&(hihhihi >= 0.70)){ 00183 dista = 10; 00184 }else if((direc == 9)&&(hihhihi >= 0.70)){ 00185 dista = 10; 00186 }else if((direc == 10)&&(hihhihi >= 0.60)){ 00187 dista = 10; 00188 }else if((direc == 11)&&(hihhihi >= 0.60)){ 00189 dista = 10; 00190 }else if((direc == 12)&&(hihhihi >= 0.60)){ 00191 dista = 10; 00192 }else if((direc == 13)&&(hihhihi >= 0.65)){ 00193 dista = 10; 00194 }else if((direc == 14)&&(hihhihi >= 0.75)){ 00195 dista = 10; 00196 }else if((direc == 15)&&(hihhihi >= 1.0)){ 00197 dista = 10; 00198 } 00199 } 00200 00201 int count = 0; 00202 00203 for(int i=0;i<DIREC_IR;i++){ 00204 if(ir_value[i])count++; 00205 } 00206 00207 if(count) IR_found = 1; 00208 else IR_found = 0; 00209 00210 00211 /******** distance end *******/ 00212 00213 past_direc = direc; 00214 00215 direction = direc; 00216 Distance = dista; 00217 00218 //printf("derection:%d distance:%d\n",direction,Distance); 00219 } 00220 } 00221 }
Generated on Tue Jul 12 2022 12:46:46 by
1.7.2