robocup

Dependencies:   HMC6352 PID mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers IR.cpp Source File

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 }