yusuke takahashi
/
Ball_fanction
IRIR
main.cpp
- Committer:
- yusuke_robocup
- Date:
- 2013-03-05
- Revision:
- 1:d6c8be12a3de
- Parent:
- 0:0c7c6eefafe4
- Child:
- 2:22b099fbdd14
File content as of revision 1:d6c8be12a3de:
#include "mbed.h" #define IR_TIME_NOTFOUND 833 /* 見つけられなかったと判断するまでの時間(単位:us) */ #define IR_COUNTMAX 487 /*最大パルス幅 パルスの存在しうる最大時間は487us*/ #define ALL_IR 9 #define DIRECTION 16 #define TERM 0 #define SWAP(type,a,b) { type temp = a; a = b; b = temp; } Serial pc(USBTX, USBRX); // tx, rx Timer timer_ir; /* 赤外線用タイマー */ BusOut myleds(p18, p17, p16, p15, p14, p13, p12, p11); //出力用ポート BusOut mbedleds(LED4,LED3,LED2,LED1); /* 赤外線センサに使うpinを配列に格納 */ PinName ir_num[ALL_IR] = { p13, p14, p15, p16, p17, p18, p19, p20, p30 }; int Convert_Direction[DIRECTION] = { 90, 67, 45, 22, 0, 337, 315, 292, 270, 247, 225, 202, 180, 157, 135, 112 }; int moving_ave(int data,int active_ir) { static int tmp[14][ALL_IR]= {{0,0}}; static int sum[ALL_IR] = {0}; int count = 0; sum[active_ir] -= tmp[13][active_ir]; sum[active_ir] += data; tmp[13][active_ir] = tmp[12][active_ir]; tmp[12][active_ir] = tmp[11][active_ir]; tmp[11][active_ir] = tmp[10][active_ir]; tmp[10][active_ir] = tmp[9][active_ir]; tmp[9][active_ir] = tmp[8][active_ir]; tmp[8][active_ir] = tmp[7][active_ir]; tmp[7][active_ir] = tmp[6][active_ir]; tmp[6][active_ir] = tmp[5][active_ir]; tmp[5][active_ir] = tmp[4][active_ir]; tmp[4][active_ir] = tmp[3][active_ir]; tmp[3][active_ir] = tmp[2][active_ir]; tmp[2][active_ir] = tmp[1][active_ir]; tmp[1][active_ir] = tmp[0][active_ir]; tmp[0][active_ir] = data; for(int i=0;i<14;i++){ if(tmp[i][active_ir])count++; } return sum[active_ir]/count; } void IR_Position(int* direction,int* distance){ 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 ); 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<ALL_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]>0)&&(ir_value[youso_min]<=25 + TERM)){ dista = 10; }else if((ir_value[youso_min]>25 + TERM)&&(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; } if(dista <= 30){ if((direc == 0)&&(ir_value[8] <= 30)){ dista = 10; }else if((direc == 1)&&(ir_value[8] <= 30 )){ dista = 10; }else if((direc == 2)&&(ir_value[8] <= 40 )){ dista = 10; }else if((direc == 3)&&(ir_value[8] <= 43)){ dista = 10; }else if((direc == 4)&&(ir_value[8] <= 42)){ dista = 10; }else if((direc == 5)&&(ir_value[8] <= 42)){ dista = 10; }else if((direc == 6)&&(ir_value[8] <= 42)){ dista = 10; }else if((direc == 7)&&(ir_value[8] <= 40)){ dista = 10; }else if((direc == 8)&&(ir_value[8] <= 38)){ dista = 10; }else if((direc == 9)&&(ir_value[8] <= 34)){ dista = 10; }else if((direc == 10)&&(ir_value[8] <= 40)){ dista = 10; }else if((direc == 11)&&(ir_value[8] <= 42)){ dista = 10; }else if((direc == 12)&&(ir_value[8] <= 39)){ dista = 10; }else if((direc == 13)&&(ir_value[8] <= 40)){ dista = 10; }else if((direc == 14)&&(ir_value[8] <= 41)){ dista = 10; }else if((direc == 15)&&(ir_value[8] <= 33)){ dista = 10; } } /******** distance end *******/ *direction = Convert_Direction[direc]; *distance = dista; } } } int main () { pc.baud(9600); int derection = 0; int distance = 0; for(;;){ IR_Position(&derection,&distance); pc.printf("derection:%d distance:%d\n",derection,distance); } }