yusuke takahashi
/
BallTest_one
ikko
main.cpp@0:dddf16147ff2, 2013-03-14 (annotated)
- Committer:
- yusuke_robocup
- Date:
- Thu Mar 14 06:16:27 2013 +0000
- Revision:
- 0:dddf16147ff2
- Child:
- 1:d9b990411155
ikko
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yusuke_robocup | 0:dddf16147ff2 | 1 | #include "mbed.h" |
yusuke_robocup | 0:dddf16147ff2 | 2 | |
yusuke_robocup | 0:dddf16147ff2 | 3 | #define IR_TIME_NOTFOUND 833 /* 見つけられなかったと判断するまでの時間(単位:us) */ |
yusuke_robocup | 0:dddf16147ff2 | 4 | #define IR_COUNTMAX 487 /*最大パルス幅 パルスの存在しうる最大時間は487us*/ |
yusuke_robocup | 0:dddf16147ff2 | 5 | #define ALL_IR 8 |
yusuke_robocup | 0:dddf16147ff2 | 6 | |
yusuke_robocup | 0:dddf16147ff2 | 7 | Serial pc(USBTX, USBRX); // tx, rx |
yusuke_robocup | 0:dddf16147ff2 | 8 | |
yusuke_robocup | 0:dddf16147ff2 | 9 | Timer timer_ir; /* 赤外線用タイマー */ |
yusuke_robocup | 0:dddf16147ff2 | 10 | |
yusuke_robocup | 0:dddf16147ff2 | 11 | BusOut myleds(p18, p17, p16, p15, p14, p13, p12, p11); //出力用ポート |
yusuke_robocup | 0:dddf16147ff2 | 12 | |
yusuke_robocup | 0:dddf16147ff2 | 13 | BusOut mbedleds(LED4,LED3,LED2,LED1); |
yusuke_robocup | 0:dddf16147ff2 | 14 | |
yusuke_robocup | 0:dddf16147ff2 | 15 | /* 赤外線センサに使うpinを配列に格納 */ |
yusuke_robocup | 0:dddf16147ff2 | 16 | PinName ir_num[ALL_IR] = { |
yusuke_robocup | 0:dddf16147ff2 | 17 | p19, |
yusuke_robocup | 0:dddf16147ff2 | 18 | p20, |
yusuke_robocup | 0:dddf16147ff2 | 19 | p25, |
yusuke_robocup | 0:dddf16147ff2 | 20 | p26, |
yusuke_robocup | 0:dddf16147ff2 | 21 | p27, |
yusuke_robocup | 0:dddf16147ff2 | 22 | p28 |
yusuke_robocup | 0:dddf16147ff2 | 23 | }; |
yusuke_robocup | 0:dddf16147ff2 | 24 | |
yusuke_robocup | 0:dddf16147ff2 | 25 | DigitalIn sensor_ir(p19); |
yusuke_robocup | 0:dddf16147ff2 | 26 | |
yusuke_robocup | 0:dddf16147ff2 | 27 | unsigned int moving_ave(unsigned int data) |
yusuke_robocup | 0:dddf16147ff2 | 28 | { |
yusuke_robocup | 0:dddf16147ff2 | 29 | static unsigned int tmp[14] = {0}; |
yusuke_robocup | 0:dddf16147ff2 | 30 | static unsigned int sum = 0; |
yusuke_robocup | 0:dddf16147ff2 | 31 | uint8_t i; |
yusuke_robocup | 0:dddf16147ff2 | 32 | uint8_t count; |
yusuke_robocup | 0:dddf16147ff2 | 33 | |
yusuke_robocup | 0:dddf16147ff2 | 34 | sum -= tmp[13]; |
yusuke_robocup | 0:dddf16147ff2 | 35 | sum += data; |
yusuke_robocup | 0:dddf16147ff2 | 36 | tmp[13] = tmp[12]; |
yusuke_robocup | 0:dddf16147ff2 | 37 | tmp[12] = tmp[11]; |
yusuke_robocup | 0:dddf16147ff2 | 38 | tmp[11] = tmp[10]; |
yusuke_robocup | 0:dddf16147ff2 | 39 | tmp[10] = tmp[9]; |
yusuke_robocup | 0:dddf16147ff2 | 40 | tmp[9] = tmp[8]; |
yusuke_robocup | 0:dddf16147ff2 | 41 | tmp[8] = tmp[7]; |
yusuke_robocup | 0:dddf16147ff2 | 42 | tmp[7] = tmp[6]; |
yusuke_robocup | 0:dddf16147ff2 | 43 | tmp[6] = tmp[5]; |
yusuke_robocup | 0:dddf16147ff2 | 44 | tmp[5] = tmp[4]; |
yusuke_robocup | 0:dddf16147ff2 | 45 | tmp[4] = tmp[3]; |
yusuke_robocup | 0:dddf16147ff2 | 46 | tmp[3] = tmp[2]; |
yusuke_robocup | 0:dddf16147ff2 | 47 | tmp[2] = tmp[1]; |
yusuke_robocup | 0:dddf16147ff2 | 48 | tmp[1] = tmp[0]; |
yusuke_robocup | 0:dddf16147ff2 | 49 | tmp[0] = data; |
yusuke_robocup | 0:dddf16147ff2 | 50 | |
yusuke_robocup | 0:dddf16147ff2 | 51 | return sum/10; |
yusuke_robocup | 0:dddf16147ff2 | 52 | } |
yusuke_robocup | 0:dddf16147ff2 | 53 | |
yusuke_robocup | 0:dddf16147ff2 | 54 | int main () { |
yusuke_robocup | 0:dddf16147ff2 | 55 | unsigned int active_ir = 0; /* 今回更新する赤外線の番号 */ |
yusuke_robocup | 0:dddf16147ff2 | 56 | unsigned int memory_ir = 0; /*赤外線時間カウンタ*/ |
yusuke_robocup | 0:dddf16147ff2 | 57 | unsigned int flag_ir = 0; |
yusuke_robocup | 0:dddf16147ff2 | 58 | |
yusuke_robocup | 0:dddf16147ff2 | 59 | for(;;){ |
yusuke_robocup | 0:dddf16147ff2 | 60 | flag_ir = 1; |
yusuke_robocup | 0:dddf16147ff2 | 61 | |
yusuke_robocup | 0:dddf16147ff2 | 62 | //DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */ |
yusuke_robocup | 0:dddf16147ff2 | 63 | |
yusuke_robocup | 0:dddf16147ff2 | 64 | timer_ir.start(); /* タイマー起動 */ |
yusuke_robocup | 0:dddf16147ff2 | 65 | |
yusuke_robocup | 0:dddf16147ff2 | 66 | if(sensor_ir){ /* もし立ち上がっていたら */ |
yusuke_robocup | 0:dddf16147ff2 | 67 | while(sensor_ir){ /* 立ち下がるまで待つ */ |
yusuke_robocup | 0:dddf16147ff2 | 68 | if(timer_ir.read_us() >= IR_TIME_NOTFOUND){ |
yusuke_robocup | 0:dddf16147ff2 | 69 | flag_ir = 0; |
yusuke_robocup | 0:dddf16147ff2 | 70 | break; /* 立ち上がっている時間が指定時間越えたらブレイク */ |
yusuke_robocup | 0:dddf16147ff2 | 71 | } |
yusuke_robocup | 0:dddf16147ff2 | 72 | } |
yusuke_robocup | 0:dddf16147ff2 | 73 | } |
yusuke_robocup | 0:dddf16147ff2 | 74 | |
yusuke_robocup | 0:dddf16147ff2 | 75 | timer_ir.stop(); /* タイマー停止 */ |
yusuke_robocup | 0:dddf16147ff2 | 76 | timer_ir.reset(); /* タイマーリセット */ |
yusuke_robocup | 0:dddf16147ff2 | 77 | |
yusuke_robocup | 0:dddf16147ff2 | 78 | /*ボールが指定時間内に見つかっていたら*/ |
yusuke_robocup | 0:dddf16147ff2 | 79 | if(flag_ir){ |
yusuke_robocup | 0:dddf16147ff2 | 80 | timer_ir.start(); /* タイマー起動 */ |
yusuke_robocup | 0:dddf16147ff2 | 81 | |
yusuke_robocup | 0:dddf16147ff2 | 82 | while(!(sensor_ir)){ /* 立ち上がるまで待つ */ |
yusuke_robocup | 0:dddf16147ff2 | 83 | if(timer_ir.read_us() >= IR_TIME_NOTFOUND){ |
yusuke_robocup | 0:dddf16147ff2 | 84 | break; /* 立ち上がっている時間が指定時間越えたらブレイク */ |
yusuke_robocup | 0:dddf16147ff2 | 85 | } |
yusuke_robocup | 0:dddf16147ff2 | 86 | } |
yusuke_robocup | 0:dddf16147ff2 | 87 | |
yusuke_robocup | 0:dddf16147ff2 | 88 | memory_ir = timer_ir.read_us(); |
yusuke_robocup | 0:dddf16147ff2 | 89 | |
yusuke_robocup | 0:dddf16147ff2 | 90 | while(1){ |
yusuke_robocup | 0:dddf16147ff2 | 91 | |
yusuke_robocup | 0:dddf16147ff2 | 92 | unsigned int counter_noise; |
yusuke_robocup | 0:dddf16147ff2 | 93 | |
yusuke_robocup | 0:dddf16147ff2 | 94 | if(!(sensor_ir)){ |
yusuke_robocup | 0:dddf16147ff2 | 95 | myleds = moving_ave( (timer_ir.read_us()-memory_ir)/10 ); /* パルス幅を記録 */ |
yusuke_robocup | 0:dddf16147ff2 | 96 | //ir_width[active_ir] = timer_ir.read_us()-memory_ir; /* パルス幅を記録 */ |
yusuke_robocup | 0:dddf16147ff2 | 97 | //myleds = (timer_ir.read_us()-memory_ir)/10; |
yusuke_robocup | 0:dddf16147ff2 | 98 | |
yusuke_robocup | 0:dddf16147ff2 | 99 | myleds = 0xFF; |
yusuke_robocup | 0:dddf16147ff2 | 100 | |
yusuke_robocup | 0:dddf16147ff2 | 101 | pc.printf("%d\n",moving_ave( (timer_ir.read_us()-memory_ir)/10 )); |
yusuke_robocup | 0:dddf16147ff2 | 102 | |
yusuke_robocup | 0:dddf16147ff2 | 103 | wait(0.01); |
yusuke_robocup | 0:dddf16147ff2 | 104 | break; |
yusuke_robocup | 0:dddf16147ff2 | 105 | }else{ |
yusuke_robocup | 0:dddf16147ff2 | 106 | /*ノイズ対策(ノイズで一瞬だけ立ち上がる可能性があるから)*/ |
yusuke_robocup | 0:dddf16147ff2 | 107 | //counter_noise = timer_ir.read_us(); /* 経過時間を保存 */ |
yusuke_robocup | 0:dddf16147ff2 | 108 | //while(timer_ir.read_us()-counter_noise <= 6){}; /* 6us間待つ */ |
yusuke_robocup | 0:dddf16147ff2 | 109 | //if(!(sensor_ir)){ /* もし立ち下がっていたら */ |
yusuke_robocup | 0:dddf16147ff2 | 110 | // flag_ir = 0; //iminasi |
yusuke_robocup | 0:dddf16147ff2 | 111 | |
yusuke_robocup | 0:dddf16147ff2 | 112 | // timer_ir.stop(); /* タイマー停止 */ |
yusuke_robocup | 0:dddf16147ff2 | 113 | // timer_ir.reset(); /* タイマーリセット */ |
yusuke_robocup | 0:dddf16147ff2 | 114 | |
yusuke_robocup | 0:dddf16147ff2 | 115 | // break; |
yusuke_robocup | 0:dddf16147ff2 | 116 | //} |
yusuke_robocup | 0:dddf16147ff2 | 117 | } |
yusuke_robocup | 0:dddf16147ff2 | 118 | } |
yusuke_robocup | 0:dddf16147ff2 | 119 | }else{ |
yusuke_robocup | 0:dddf16147ff2 | 120 | /*ボールが見つかっていない場合*/ |
yusuke_robocup | 0:dddf16147ff2 | 121 | //ir_width[active_ir] = 0; |
yusuke_robocup | 0:dddf16147ff2 | 122 | myleds = 0x00; /* パルス幅を記録 */ |
yusuke_robocup | 0:dddf16147ff2 | 123 | } |
yusuke_robocup | 0:dddf16147ff2 | 124 | timer_ir.stop(); /* タイマー停止 */ |
yusuke_robocup | 0:dddf16147ff2 | 125 | timer_ir.reset(); /* タイマーリセット */ |
yusuke_robocup | 0:dddf16147ff2 | 126 | |
yusuke_robocup | 0:dddf16147ff2 | 127 | memory_ir = 0; |
yusuke_robocup | 0:dddf16147ff2 | 128 | |
yusuke_robocup | 0:dddf16147ff2 | 129 | /*active_ir++; 次の赤外線センサへ(読み取る赤外線センサを順番に回す) |
yusuke_robocup | 0:dddf16147ff2 | 130 | if( active_ir >= NUM_IR ){ |
yusuke_robocup | 0:dddf16147ff2 | 131 | active_ir = 0; 6個全ての赤外線センサをまわったら一番初めに戻る |
yusuke_robocup | 0:dddf16147ff2 | 132 | }*/ |
yusuke_robocup | 0:dddf16147ff2 | 133 | } |
yusuke_robocup | 0:dddf16147ff2 | 134 | } |