yusuke takahashi
/
Ball_fanction
IRIR
Diff: main.cpp
- Revision:
- 0:0c7c6eefafe4
- Child:
- 1:d6c8be12a3de
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Mar 04 07:50:18 2013 +0000 @@ -0,0 +1,220 @@ +#include "mbed.h" + +#define IR_TIME_NOTFOUND 833 /* 見つけられなかったと判断するまでの時間(単位:us) */ +#define IR_COUNTMAX 487 /*最大パルス幅 パルスの存在しうる最大時間は487us*/ +#define ALL_IR 8 +#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 +}; + +int Convert_Direction[DIRECTION] = { + 0, + 23, + 45, + 68, + 90, + 113, + 135, + 158, + 180, + -158, + -135, + -113, + -90, + -68, + -45, + -23 +}; + +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.83)&&(youso_min != 0)) { + direc = youso_min*2-1; + }else if((hiritu <= 0.83)&&(youso_min == 0)){ + direc = 15; + }else if(hiritu >= 1.17) { + direc = youso_min*2+1; + } else { + direc = youso_min*2; + } + + /******* 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; + } + + /******** 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); + } +}