robocup
Dependencies: HMC6352 PID mbed
IR.cpp
- Committer:
- akudohune
- Date:
- 2013-03-08
- Revision:
- 0:13ab960fc61f
File content as of revision 0:13ab960fc61f:
#include "mbed.h" #include "IR.h" extern Timer timer_ir; extern Serial pc; // tx, rx int direction = 0; int Distance = 0; int IR_found; void IR_Position(){ int ir_value[ALL_IR+100] = {0}; int active_ir = 0; /* 今回更新する赤外線の番号 */ int memory_ir = 0; /*赤外線時間カウンタ*/ int flag_ir = 0; int value = 0; for(int i=0; i<ALL_IR; i++) { flag_ir = 1; DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */ timer_ir.start(); /* タイマー起動 */ if(sensor_ir) { /* もし立ち上がっていたら */ while(sensor_ir) { /* 立ち下がるまで待つ */ if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { flag_ir = 0; break; /* 立ち上がっている時間が指定時間越えたらブレイク */ } } } timer_ir.stop(); /* タイマー停止 */ timer_ir.reset(); /* タイマーリセット */ if(flag_ir) { timer_ir.start(); /* タイマー起動 */ while(!(sensor_ir)) { /* 立ち上がるまで待つ */ if(timer_ir.read_us() >= IR_TIME_NOTFOUND) { flag_ir = 0; break; /* 立ち上がっている時間が指定時間越えたらブレイク */ } } } /*ボールが指定時間内に見つかっていたら*/ if(flag_ir) { memory_ir = timer_ir.read_us(); while(1) { if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break; if(!(sensor_ir)) { //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir ); value = (timer_ir.read_us()-memory_ir)/10; break; } } } else { /*ボールが見つかっていない場合*/ value = 0; } timer_ir.stop(); /* タイマー停止 */ timer_ir.reset(); /* タイマーリセット */ memory_ir = 0; ir_value[active_ir] = value; //direction array active_ir++; if( active_ir >= ALL_IR) { active_ir = 0; /***********direction***********/ int min = 100,youso_min = 100; for(int i = 0; i<DIREC_IR; i++) { if((ir_value[i]<min)&&(ir_value[i])) { min = ir_value[i]; youso_min = i; } } double hiritu = 0; int direc = 0; if(youso_min == 0) { hiritu = (double)ir_value[7]/(double)ir_value[1]; } else if(youso_min ==7) { hiritu = (double)ir_value[6]/(double)ir_value[0]; } else { hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1]; } if((hiritu <= 0.85)&&(youso_min != 0)) { direc = youso_min*2-1; }else if((hiritu <= 0.85)&&(youso_min == 0)){ direc = 15; }else if(hiritu >= 1.15) { direc = youso_min*2+1; } else { direc = youso_min*2; } /*if(youso_min == 0){ direc = 0; }*/ /******* direction end *******/ /******* distance *******/ int dista; if((ir_value[youso_min]<=28 + TERM)) { dista = 30; } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) { dista = 90; } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) { dista = 120; } else if( ir_value[youso_min]>40 + TERM) { dista = 180; } else { dista = 0; } int count_ir = 0,total_ir = 0; for(int i=0; i<DIREC_IR; i++){ if(ir_value[i]){ total_ir += ir_value[i]; count_ir++; } } double hihhihi = 0; hihhihi = (double)ir_value[youso_min]/(double)ir_value[8]; if((direc == 0)&&(hihhihi >= 0.80)){ dista = 10; }else if((direc == 1)&&(hihhihi >= 0.80)){ dista = 10; }else if((direc == 2)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 3)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 4)&&(hihhihi >= 0.80)){ dista = 10; }else if((direc == 5)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 6)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 7)&&(hihhihi >= 0.80)){ dista = 10; }else if((direc == 8)&&(hihhihi >= 0.80)){ dista = 10; }else if((direc == 9)&&(hihhihi >= 0.80)){ dista = 10; }else if((direc == 10)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 11)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 12)&&(hihhihi >= 0.80)){ dista = 10; }else if((direc == 13)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 14)&&(hihhihi >= 0.65)){ dista = 10; }else if((direc == 15)&&(hihhihi >= 0.80)){ dista = 10; } int count = 0; for(int i=0;i<DIREC_IR;i++){ if(ir_value[i])count++; } if(count) IR_found = 1; else IR_found = 0; /******** distance end *******/ direction = direc; Distance = dista; //printf("derection:%d distance:%d\n",direction,Distance); } } }